/* File : fixed.c
* Autor : Damizean
* Description : Fixed-point maths library.
* Last update : 14 / 08 / 2005
*/
#include "fixed.h"
#include <math.h>
// --- Define variables -----------------------------------------
fixed MATHS_PI;
fixed MATHS_PI_OVER_2;
fixed MATHS_E;
fixed MATHS_SINK1;
fixed MATHS_SINK2;
fixed MATHS_COSK1;
fixed MATHS_COSK2;
fixed MATHS_TANK1;
fixed MATHS_TANK2;
void maths_init ()
{
// --- Init math vars -------
MATHS_PI = double_to_fixed ( 3.14159265f );
MATHS_PI_OVER_2= double_to_fixed ( 1.57079 );
MATHS_E = double_to_fixed ( 2.71827 );
MATHS_SINK1 = double_to_fixed ( 0.00759 );
MATHS_SINK2 = double_to_fixed ( 0.16604 );
MATHS_COSK1 = double_to_fixed ( 0.03552 );
MATHS_COSK2 = double_to_fixed ( 0.49668 );
MATHS_TANK1 = double_to_fixed ( 0.20329 );
MATHS_TANK2 = double_to_fixed ( 0.31753 );
}
// --- Convert variables to fixed point -------------------------
fixed int_to_fixed ( int value )
{
return (fixed) ( value << fixedshifts );
}
fixed float_to_fixed ( float value )
{
return (fixed) ((float) value * ((fixed) 1 << fixedshifts ) );
}
fixed double_to_fixed ( double value )
{
return (fixed) ((double) value * ((fixed) 1 << fixedshifts ) );
}
// --- Convert fixed point to other variables -----------------
int fixed_to_int ( fixed value )
{
return (int) ( value >> fixedshifts );
}
float fixed_to_float ( fixed value )
{
return ( (float) value / (float) ((fixed) 1 << fixedshifts ) );
}
double fixed_to_double ( fixed value )
{
return ( (double) value / (double) ((fixed) 1 << fixedshifts ) );
}
// --- Operations ---------------------------------------
fixed fixed_add ( fixed pFixed1, fixed pFixed2 )
{
return ( pFixed1 + pFixed2 );
}
fixed fixed_sub ( fixed pFixed1, fixed pFixed2 )
{
return ( pFixed1 - pFixed2 );
}
fixed fixed_mul ( fixed pFixed1, fixed pFixed2 )
{
long long tmp;
tmp = ((pFixed1 * pFixed2 ) >> fixedshifts);
return (fixed) tmp;
}
fixed fixed_div ( fixed pFixed1, fixed pFixed2 )
{
long long tmp;
tmp = (pFixed1 << fixedshifts) / pFixed2;
return (fixed) tmp;
}
// --- Others -------------------------------------------
fixed fixed_degtorad ( fixed value )
{
return fixed_mul( value, fixed_div( MATHS_PI,int_to_fixed( 180) ) );
}
fixed fixed_radtodeg ( fixed value )
{
return fixed_mul( value, fixed_div( int_to_fixed( 180 ), MATHS_PI ) );
}
fixed fixed_sin ( fixed value )
{
fixed sign, sqr, result;
sign = 1;
// ---- Set up the range ----------------------
if ( (value > MATHS_PI_OVER_2 ) && ( value <= MATHS_PI ))
{
value = MATHS_PI - value;
} else if ((value > MATHS_PI) && (value <= (MATHS_PI + MATHS_PI_OVER_2)))
{
value = value - MATHS_PI;
sign = -1;
} else if (value > (MATHS_PI + MATHS_PI_OVER_2))
{
value = (MATHS_PI<<1)-value;
sign = -1;
}
// ----- Calculate ----------------------------
sqr = fixed_sqr ( value );
result = MATHS_SINK1;
result = fixed_mul ( result, sqr );
result -= MATHS_SINK2;
result = fixed_mul ( result, sqr );
result += (1 << fixedshifts);
result = fixed_mul ( result, value );
return sign * result;
}
fixed fixed_cos ( fixed value )
{
fixed sign, sqr, result;
sign = 1;
// ---- Set up the range ----------------------
if ( (value > MATHS_PI_OVER_2 ) && ( value <= MATHS_PI ))
{
value = MATHS_PI - value;
sign = -1;
} else if ((value > MATHS_PI_OVER_2) && (value <= (MATHS_PI + MATHS_PI_OVER_2)))
{
value = value - MATHS_PI;
sign = -1;
} else if (value > (MATHS_PI + MATHS_PI_OVER_2))
{
value = (MATHS_PI<<1)-value;
}
// ----- Calculate ----------------------------
sqr = fixed_sqr ( value );
result = MATHS_COSK1;
result = fixed_mul ( result, sqr );
result -= MATHS_COSK2;
result = fixed_mul ( result, sqr );
result += (1 << fixedshifts);
return sign * result;
}
fixed fixed_tan ( fixed value )
{
fixed sqr, result;
sqr = fixed_sqr ( value );
result = MATHS_TANK1;
result = fixed_mul ( result, sqr );
result += MATHS_TANK2;
result = fixed_mul ( result, sqr );
result += (1 << fixedshifts);
result = fixed_mul ( result, value );
return result;
}
fixed fixed_sqr ( fixed value )
{
return fixed_mul( value, value );
}
fixed fixed_sqrt ( fixed value )
{
fixed square = (value + (1 << fixedshifts) ) >> 1;
int i;
for ( i = 0; i < 8; i ++ )
square = (square + fixed_div( value, square ) ) >> 1;
return square;
}
Marcadores