forked from glidernet/diy-tracker
-
Notifications
You must be signed in to change notification settings - Fork 0
/
intmath.cpp
157 lines (139 loc) · 6.44 KB
/
intmath.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include "intmath.h"
static const uint32_t SinePoints=256;
// static const uint32_t SineScale=0x80000000;
static const int32_t SineTable[SinePoints/4+1] =
{ 0x00000000, 0x03242ABF, 0x0647D97C, 0x096A9049, 0x0C8BD35E, 0x0FAB272B, 0x12C8106F, 0x15E21445,
0x18F8B83C, 0x1C0B826A, 0x1F19F97B, 0x2223A4C5, 0x25280C5E, 0x2826B928, 0x2B1F34EB, 0x2E110A62,
0x30FBC54D, 0x33DEF287, 0x36BA2014, 0x398CDD32, 0x3C56BA70, 0x3F1749B8, 0x41CE1E65, 0x447ACD50,
0x471CECE7, 0x49B41533, 0x4C3FDFF4, 0x4EBFE8A5, 0x5133CC94, 0x539B2AF0, 0x55F5A4D2, 0x5842DD54,
0x5A82799A, 0x5CB420E0, 0x5ED77C8A, 0x60EC3830, 0x62F201AC, 0x64E88926, 0x66CF8120, 0x68A69E81,
0x6A6D98A4, 0x6C242960, 0x6DCA0D14, 0x6F5F02B2, 0x70E2CBC6, 0x72552C85, 0x73B5EBD1, 0x7504D345,
0x7641AF3D, 0x776C4EDB, 0x78848414, 0x798A23B1, 0x7A7D055B, 0x7B5D039E, 0x7C29FBEE, 0x7CE3CEB2,
0x7D8A5F40, 0x7E1D93EA, 0x7E9D55FC, 0x7F0991C4, 0x7F62368F, 0x7FA736B4, 0x7FD8878E, 0x7FF62182,
0x7FFFFFFF } ;
// get Sine from the SineTable
// Angle is 0..255 which corresponds to <0..2*PI)
int32_t IntSine(uint8_t Angle)
{ uint8_t Idx=Angle;
if(Angle&0x80) { Angle^=0x40; Idx=(-Idx); }
if(Angle&0x40) Idx=0x80-Idx;
int32_t Val=SineTable[Idx];
if(Angle&0x80) Val=(-Val);
return Val; }
// precise Sine with for 16-bit angles 2nd derivative interpolation
// max. result error is about 2.3e-7
int32_t IntSine(uint16_t Angle)
{ uint8_t Int = Angle>>8;
int32_t Frac = Angle&0x00FF;
int32_t Value = IntSine(Int); Int+=1;
int32_t Delta = (IntSine(Int)-Value)>>8;
Value += Frac*Delta;
// printf(" [%02X %02X %+11.8f] ", Int-1, Frac, (double)Value/(uint32_t)0x80000000);
int32_t Frac2 = (Frac*(Frac-0x100));
const int32_t Coeff = (int32_t)floor(2*M_PI*M_PI*0x80+0.5);
int32_t Deriv2 = (Coeff*(Value>>12));
int32_t Corr = ((Deriv2>>16)*Frac2)>>11;
Value -= Corr;
// printf("[%04X %+11.8f %+11.8f] ", -Frac2, (double)Deriv2/(uint32_t)0x80000000, (double)Corr/(uint32_t)0x80000000);
return Value; }
// precise Sine for 32-bit angles with 2nd derivative interpolation
// max. result error is about 2.3e-7
int32_t IntSine(uint32_t Angle)
{ uint8_t Int = Angle>>24;
int32_t Frac = Angle&0x00FFFFFF;
int32_t Value = IntSine(Int); Int+=1;
int32_t Delta = (IntSine(Int)-Value);
Value += ((int64_t)Frac*(int64_t)Delta)>>24;
// printf(" [%02X %06X %+11.8f] ", Int-1, Frac, (double)Value/(uint32_t)0x80000000);
int64_t Frac2 = ((int64_t)Frac*(Frac-0x1000000))>>32;
const int64_t Coeff = (int64_t)floor(2*M_PI*M_PI*0x4000000+0.5);
int64_t Deriv2 = (Coeff*Value)>>26;
int64_t Corr = (Deriv2*Frac2)>>32;
Value -= Corr;
// printf(" [%04X %+11.8f %+11.8f] ", -Frac2, (double)Deriv2/(uint32_t)0x80000000, (double)Corr/(uint32_t)0x80000000);
return Value; }
// Less precise sine for 16-bit angles
// source: http://www.coranac.com/2009/07/sines/
/// A sine approximation via a fourth-order cosine approx.
/// @param x angle (with 2^16 units/circle)
/// @return Sine value (Q12)
int16_t Isin(int16_t Angle) // input: full angle = 16-bit range
{
int32_t x=Angle;
int32_t c, y;
static const int qN= 14, qA= 12, B=19900, C=3516;
c= x<<(30-qN); // Semi-circle info into carry.
x -= 1<<qN; // sine -> cosine calc
x= x<<(31-qN); // Mask with PI
x= x>>(31-qN); // Note: SIGNED shift! (to qN)
x= (x*x)>>(2*qN-14); // x=x^2 To Q14
y= B - ((x*C)>>14); // B - x^2*C
y= (1<<qA)-((x*y)>>16); // A - x^2*(B-x^2*C)
return c>=0 ? y : -y; } // result: -4096..+4096 (max. error = +/-12)
/*
int16_t IntAtan2(int16_t Y, int16_t X)
{ uint16_t Angle=0; // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(Y<0) { Angle+=0x8000; X=(-X); Y=(-Y); } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(X<0) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(X<Y) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(X==0) return 0;
int16_t D = (((int32_t)Y<<14) + (X>>1))/ X;
int16_t DD = ((int32_t)D*(int32_t)D)>>14;
int16_t DDD = ((int32_t)DD*(int32_t)D)>>14;
// printf(" %08X %08X %08X\n", D, DD, DDD);
Angle += ((5*D)>>3) - (DDD>>3); return Angle; } // good to about 1/2 degree
*/
int16_t IntAtan2(int16_t Y, int16_t X)
{ uint16_t Angle=0; // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
const int32_t CosPi8 = 30274; // cos(PI/8)*32768
const int32_t SinPi8 = 12540; // sin(PI/8)*32768
if(Y<0) { Angle+=0x8000; X=(-X); Y=(-Y); } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(X<0) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(X<Y) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
if(X==0) return 0;
int16_t Yc = (Y<<1)+(Y>>1);
if(Y<0)
{ if(X<(-Yc))
{ int32_t NewX = CosPi8*X - SinPi8*Y;
int32_t NewY = SinPi8*X + CosPi8*Y;
X=NewX>>15; if(NewX&0x4000) X+=1;
Y=NewY>>15; if(NewY&0x4000) Y+=1;
Angle-=0x1000; }
} else // Y>=0
{ if(X<Yc)
{ int32_t NewX = CosPi8*X + SinPi8*Y;
int32_t NewY = - SinPi8*X + CosPi8*Y;
X=NewX>>15; if(NewX&0x4000) X+=1;
Y=NewY>>15; if(NewY&0x4000) Y+=1;
Angle+=0x1000; }
} // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
int16_t D = (((int32_t)Y<<14) + (X>>1))/ X;
// int16_t D = ((int32_t)Y<<14) / X;
int16_t DD = ((int32_t)D*(int32_t)D)>>14;
int16_t DDD = ((int32_t)DD*(int32_t)D)>>14;
// printf(" %08X %08X %08X\n", D, DD, DDD);
Angle += ((5*D)>>3) - (DDD>>3);
return Angle; } // good to about 1/6 degree
/*
// integer square root
uint32_t IntSqrt(uint32_t Inp)
{ uint32_t Out = 0;
uint32_t Mask = 0x40000000;
while(Mask>Inp) Mask>>=2;
while(Mask)
{ if(Inp >= (Out+Mask))
{ Inp -= Out+Mask; Out += Mask<<1; }
Out>>=1; Mask>>=2; }
if(Inp>Out) Out++;
return Out; }
uint64_t IntSqrt(uint64_t Inp)
{ uint64_t Out = 0;
uint64_t Mask = 0x4000000000000000;
while(Mask>Inp) Mask>>=2;
while(Mask)
{ if(Inp >= (Out+Mask))
{ Inp -= Out+Mask; Out += Mask<<1; }
Out>>=1; Mask>>=2; }
if(Inp>Out) Out++;
return Out; }
*/