184 lines
4.1 KiB
C++
184 lines
4.1 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;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
libnum::num32 sqrt_num32(libnum::num32 v) {
|
||
|
uint32_t t, q, b, r;
|
||
|
r = v.v;
|
||
|
b = 0x40000000;
|
||
|
q = 0;
|
||
|
while (b > 0x40) {
|
||
|
t = q + b;
|
||
|
if (r >= t) {
|
||
|
r -= t;
|
||
|
q = t + b;
|
||
|
}
|
||
|
r <<= 1;
|
||
|
b >>= 1;
|
||
|
}
|
||
|
q >>= 8;
|
||
|
libnum::num32 ret;
|
||
|
ret.v = q;
|
||
|
return ret;
|
||
|
}
|
||
|
|
||
|
/* TO DO : rework these functions for sine and cosine calculation */
|
||
|
|
||
|
libnum::num32 cos_num32(libnum::num32 angle) {
|
||
|
// Taylor serie for cos(x) = 1 - x²/2! + x⁴/4! + x⁶/6! + x⁸/8! + ...
|
||
|
|
||
|
// Cosine function is even
|
||
|
if (angle < libnum::num32(0))
|
||
|
return cos_num32(-angle);
|
||
|
|
||
|
// Look for an angle in the range [0, 2*pi [
|
||
|
libnum::num32 anglereduced = angle;
|
||
|
while (anglereduced >= libnum::num32(2 * PI))
|
||
|
anglereduced -= libnum::num32(2 * PI);
|
||
|
|
||
|
// Exploit the symetry for angle and angle+Pi to reduce the order of the
|
||
|
// limited developpement
|
||
|
if (anglereduced >= libnum::num(PI))
|
||
|
return -cos_num32(anglereduced - libnum::num(PI));
|
||
|
|
||
|
libnum::num32 sum = libnum::num32(1);
|
||
|
libnum::num32 angle2 = anglereduced * anglereduced;
|
||
|
|
||
|
// set first value of the Taylor serie : x⁰/0! = 1/1
|
||
|
libnum::num32 numerator = libnum::num32(1);
|
||
|
libnum::num32 denominator = libnum::num32(1);
|
||
|
|
||
|
for (int i = 2; i <= 8; i += 2) {
|
||
|
numerator *= (-angle2);
|
||
|
denominator *= libnum::num32(i - 1) * libnum::num32(i);
|
||
|
sum += (numerator / denominator);
|
||
|
}
|
||
|
|
||
|
return sum;
|
||
|
}
|
||
|
|
||
|
libnum::num32 sin_num32(libnum::num32 angle) {
|
||
|
// Taylor serie for cos(x) = x/1! - x³/3! + x⁵/5! - x⁷/7! + x⁹/9! + ...
|
||
|
|
||
|
// Sine function is odd
|
||
|
if (angle < libnum::num32(0))
|
||
|
return -sin_num32(-angle);
|
||
|
|
||
|
// Look for an angle in the range [0, 2*pi [
|
||
|
libnum::num32 anglereduced = angle;
|
||
|
while (anglereduced >= libnum::num32(2 * PI))
|
||
|
anglereduced -= libnum::num32(2 * PI);
|
||
|
|
||
|
// Exploit the symetry for angle and angle+Pi to reduce the order of the
|
||
|
// limited developpement
|
||
|
if (anglereduced >= libnum::num(PI))
|
||
|
return -sin_num32(anglereduced - libnum::num(PI));
|
||
|
|
||
|
libnum::num32 sum = anglereduced;
|
||
|
libnum::num32 angle2 = anglereduced * anglereduced;
|
||
|
|
||
|
// set first value of the Taylor serie : x¹/1! = x/1
|
||
|
libnum::num32 numerator = anglereduced;
|
||
|
libnum::num32 denominator = libnum::num32(1);
|
||
|
|
||
|
for (int i = 2; i <= 8; i += 2) {
|
||
|
numerator *= (-angle2);
|
||
|
denominator *= libnum::num32(i) * libnum::num32(i + 1);
|
||
|
sum += (numerator / denominator);
|
||
|
}
|
||
|
|
||
|
return sum;
|
||
|
}
|