94 lines
1.8 KiB
C++
94 lines
1.8 KiB
C++
#include "fast_trig.h"
|
|
#include "num/num.h"
|
|
|
|
|
|
static libnum::num cosTable[360];
|
|
static libnum::num sinTable[360];
|
|
static bool is_fast_trig_initialised = false;
|
|
|
|
void Fast_Trig_Init( void )
|
|
{
|
|
for(int u=0; u<360; u++)
|
|
{
|
|
cosTable[u] = libnum::num( cos( u * PI / 180 ) );
|
|
sinTable[u] = libnum::num( sin( u * PI / 180 ) );
|
|
}
|
|
is_fast_trig_initialised = true;
|
|
}
|
|
|
|
|
|
libnum::num FastCosInt( int16_t angle )
|
|
{
|
|
if (!is_fast_trig_initialised) Fast_Trig_Init();
|
|
|
|
if (angle>=0 and angle<360) return cosTable[ angle ];
|
|
else
|
|
{
|
|
int16_t input = angle;
|
|
if (input<0)
|
|
{
|
|
while (input<0) input+=360;
|
|
return cosTable[ input ];
|
|
}
|
|
else
|
|
{
|
|
while (input>=360) input-=360;
|
|
return cosTable[ input ];
|
|
}
|
|
}
|
|
}
|
|
|
|
libnum::num FastSinInt( int16_t angle )
|
|
{
|
|
if (!is_fast_trig_initialised) Fast_Trig_Init();
|
|
|
|
if (angle>=0 and angle<360) return sinTable[ angle ];
|
|
else
|
|
{
|
|
int16_t input = angle;
|
|
if (input<0)
|
|
{
|
|
while (input<0) input+=360;
|
|
return sinTable[ input ];
|
|
}
|
|
else
|
|
{
|
|
while (input>=360) input-=360;
|
|
return sinTable[ input ];
|
|
}
|
|
}
|
|
}
|
|
|
|
libnum::num FastTanInt( int16_t angle )
|
|
{
|
|
if (!is_fast_trig_initialised) Fast_Trig_Init();
|
|
|
|
int16_t input = angle;
|
|
|
|
if (input<0)
|
|
{
|
|
while (input<0) input+=360;
|
|
}
|
|
else if (input>=360)
|
|
{
|
|
while (input>=360) input-=360;
|
|
}
|
|
|
|
libnum::num value;
|
|
|
|
if (input==90)
|
|
{
|
|
value.v = INT32_MAX;
|
|
return value;
|
|
}
|
|
else if (input==270)
|
|
{
|
|
value.v = INT32_MIN;
|
|
return value;
|
|
}
|
|
else
|
|
{
|
|
value = FastSinInt(input) / FastCosInt(input);
|
|
return value;
|
|
}
|
|
} |