## root/modules/cordic_math.c

/* [<][>][^][v][top][bottom][index][help] */

## DEFINITIONS

This source file includes following definitions.
```   1 /*
2 CORDIC LIBRARY
3 integer based trigonometric mathematics
4 all values scaled by 100000 (CORDIC_SCALE)
5
6 based on http://www.andreadrian.de/c-workshop/index.html
7
8 (c)2012/2013 rudi from CHDK[-DE] forum
9 License GPL 2.0
10 */
11
12 #include "stdio.h"
13 #include "stdlib.h"
14 #include "../lib/lua/luaconf.h"
15 #include "math.h"
16 #include "cordic_math.h"
17
18 fixed atan_tab[][M] = {
19     {0x1921f, 0xed63, 0x7d6d, 0x3fab, 0x1ff5, 0xffe, 0x7ff, 0x3ff, 0x1ff},
20     {0x5a0000, 0x35214e, 0x1c128e, 0xe4002, 0x72715, 0x3946f, 0x1ca54, 0xe52d, 0x7297}
21 };
22 fixed INV_GAIN_CIRCLE[] = {0x136e9, 0x136e8};   // {0.60725, 0.60724}
23 fixed FULL_CIRCLE[]     = {0xc90fd, 0x2d00000}; // {6.28318, 360.00000}
24 fixed HALF_CIRCLE[]     = {0x6487e, 0x1680000}; // {3.14159, 180.00000}
25 fixed QUART_CIRCLE[]    = {0x3243f, 0xb40000};  // {1.57079, 90.00000}
26 fixed ATAN_LIMIT        = 0x36f602be;           // 7035.00536
27 fixed FIXED_MAX         = 16383.99999;
28
29 // CORDIC main routine
30 LUALIB_API void cordic(tangle t, fcordic f, fixed *x, fixed *y, fixed *z) {
31     long *patan_tab = atan_tab[t];
32     long xstep, ystep, zstep = 1;
33     int i;
34     for (i = 0; i < N; ++i) {
35         xstep = *x >> i;
36         ystep = *y >> i;
37         if (i < M) {
38             zstep = *patan_tab;
39             ++patan_tab;
40         } else zstep >>= 1;
41         int f1 = (f) ? *y >= 0 : *z < 0;
42         *x = (f1) ? *x + ystep : *x - ystep;
43         *y = (f1) ? *y - xstep : *y + xstep;
44         *z = (f1) ? *z + zstep : *z - zstep;
45     }
46 }
47
48 // abs(INT_MIN) doesn't work
49 LUALIB_API int cordic_abs(int a) {
50     if (a < -INT_MAX) a = -INT_MAX;
51     return abs(a);
52 }
53
54 LUALIB_API int cordic_sign(int a) {
55     return (a < 0)? -1 : 1;
56 }
57
58 // a x b / c
59 LUALIB_API fixed muldivScaled(fixed a, fixed b, fixed c) {
60     double res;
61     if (c != 0) {
62         int sign = cordic_sign(a) * cordic_sign(b) / cordic_sign(c);
63         res = FLOAT(cordic_abs(a)) * FLOAT(cordic_abs(b)) / FLOAT(cordic_abs(c));
64         if (res > INT_MAX) {
65             // error overflow
66             res = INT_MAX;
67         } else { res = sign * res; }
68     } else {
69         // error div 0
70         res = 0;
71     }
72     return FIXED(res);
73 }
74
75 LUALIB_API fixed mulScaled(fixed a, fixed b) {
76     return muldivScaled(a, b, CORDIC_SCALE);
77 }
78
79 LUALIB_API fixed divScaled(fixed a, fixed b) {
80     return muldivScaled(CORDIC_SCALE, a, b);
81 }
82
83 LUALIB_API int convertToQ1(fixed *x, fixed *y) {
84     int fx = (*x >= 0), fy = (*y >= 0);
85     int q = 1;
86     if (!fx && fy) {
87         q = 2;
88     } else if (!fx && !fy) {
89         q = 3;
90     } else if (fx && !fy) {
91         q = 4;
92     }
93     *x = cordic_abs(*x);
94     *y = cordic_abs(*y);
95     return q;
96 }
97
98 LUALIB_API void convertFromQ1(fixed *x, fixed *y, int q) {
99     int fx = 1, fy = 1;
100     if ((q == 2) || (q == 3)) { fx = -1; }
101     if ((q == 3) || (q == 4)) { fy = -1; }
102     *x = fx * *x;
103     *y = fy * *y;
104 }
105
106 LUALIB_API int rotateToQ1(tangle t, fixed *phi) {
107     *phi = (*phi % FULL_CIRCLE[t] + FULL_CIRCLE[t]) % FULL_CIRCLE[t];
108     int q = *phi / QUART_CIRCLE[t] + 1;
109     switch(q) {
110         case 2: *phi = HALF_CIRCLE[t] - *phi; break;
111         case 3: *phi = *phi - HALF_CIRCLE[t]; break;
112         case 4: *phi = FULL_CIRCLE[t] - *phi; break;
113     }
114     return q;
115 }
116
117 LUALIB_API void rotateFromQ1(tangle t, fixed *phi, int q) {
118     switch(q) {
119         case 2: *phi = HALF_CIRCLE[t] - *phi; break;
120         case 3: *phi = HALF_CIRCLE[t] + *phi; break;
121         case 4: *phi = FULL_CIRCLE[t] - *phi; break;
122     }
123 }
124
125 LUALIB_API fixed cathetus(fixed x) {
126     return FIXED(sqrt((1 + FLOAT(x)) * (1 - FLOAT(x))));
127 }
128
129 // base CIRCULAR mode, ROTATE
130 LUALIB_API void sincosCordic(tangle t, fixed phi, fixed *sinphi, fixed *cosphi) {
131     int q = rotateToQ1(t, &phi);
132     fixed x = INV_GAIN_CIRCLE[t], y = 0, z = phi;
133     cordic(t, ROTATE, &x, &y, &z);
134     convertFromQ1(&x, &y, q);
135     *sinphi = y;
136     *cosphi = x;
137 }
138
139 // base CIRCULAR mode, VECTOR
140 LUALIB_API void atanhypCordic(tangle t, fixed px, fixed py, fixed *phi, fixed *hyp) {
141     int q = convertToQ1(&px, &py);
142     int f = 0;
143     while (px > ATAN_LIMIT || py > ATAN_LIMIT) {
144         px /= 2;
145         py /= 2;
146         f = 1;
147     }
148     if (px == 0 && py == 0) {
149         // error input vales
150         *phi = 0;
151         *hyp = 0;
152     } else {
153         fixed x = px, y = py, z = 0;
154         cordic(t, VECTOR, &x, &y, &z);
155         rotateFromQ1(t, &z, q);
156         //shift to [pi; -pi]
157         if ((q == 3) || (q == 4)) { z = z - FULL_CIRCLE[t]; }
158         *phi = z;
159         *hyp = (f)? 0 : x;
160     }
161 }
162
163 // functions in CIRCULAR mode, ROTATE
164 LUALIB_API fixed sinCordic(tangle t, fixed phi) {
165     fixed _sin, _cos;
166     sincosCordic(t, phi, &_sin, &_cos);
167     return _sin;
168 }
169
170 LUALIB_API fixed cosCordic(tangle t, fixed phi) {
171     fixed _sin, _cos;
172     sincosCordic(t, phi, &_sin, &_cos);
173     return _cos;
174 }
175
176 LUALIB_API fixed tanCordic(tangle t, fixed phi) {
177     fixed _sin, _cos;
178     sincosCordic(t, phi, &_sin, &_cos);
179     return divScaled(_sin, _cos);
180 }
181
182 LUALIB_API void recCordic(tangle t, fixed r, fixed theta, fixed *px, fixed *py) {
183     fixed _sin, _cos;
184     sincosCordic(t, theta, &_sin, &_cos);
185     *px = mulScaled(r, _cos);
186     *py = mulScaled(r, _sin);
187 }
188
189 // functions in CIRCULAR mode, VECTOR
190 LUALIB_API fixed asinCordic(tangle t, fixed x) {
191     fixed phi, hyp;
192     fixed _cos = cathetus(x);
193     atanhypCordic(t, _cos, x, &phi, &hyp);
194     return phi;
195 }
196
197 LUALIB_API fixed acosCordic(tangle t, fixed x) {
198     fixed phi, hyp;
199     fixed _sin = cathetus(x);
200     atanhypCordic(t, x, _sin, &phi, &hyp);
201     return phi;
202 }
203
204 LUALIB_API fixed atanCordic(tangle t, fixed x) {
205     fixed phi, hyp;
206     atanhypCordic(t, CORDIC_SCALE, x, &phi, &hyp);
207     return phi;
208 }
209
210 LUALIB_API void polCordic(tangle t, fixed px, fixed py, fixed *r, fixed *theta) {
211     fixed phi, hyp;
212     atanhypCordic(t, px, py, &phi, &hyp);
213     *theta = phi;
214     *r = mulScaled(hyp, INV_GAIN_CIRCLE[t]);
215 }
216
217 // extern functions
218 // DEG
219 LUALIB_API fixed sind(fixed phi) {
220     return sinCordic(DEG, phi);
221 }
222
223 LUALIB_API fixed cosd(fixed phi) {
224     return cosCordic(DEG, phi);
225 }
226
227 LUALIB_API fixed tand(fixed phi) {
228     return tanCordic(DEG, phi);
229 }
230
231 LUALIB_API void recd(fixed r, fixed theta, fixed *px, fixed *py) {
232     recCordic(DEG, r, theta, px, py);
233 }
234
235 LUALIB_API fixed asind(fixed x) {
236     return asinCordic(DEG, x);
237 }
238
239 LUALIB_API fixed acosd(fixed x) {
240     return acosCordic(DEG, x);
241 }
242
243 LUALIB_API fixed atand(fixed x) {
244     return atanCordic(DEG, x);
245 }
246
247 LUALIB_API void pold(fixed px, fixed py, fixed *r, fixed *theta) {
248     polCordic(DEG, px, py, r, theta);
249 }
250
251 // RAD
252 LUALIB_API fixed sinr(fixed phi) {
253     return sinCordic(RAD, phi);
254 }
255
256 LUALIB_API fixed cosr(fixed phi) {
257     return cosCordic(RAD, phi);
258 }
259
260 LUALIB_API fixed tanr(fixed phi) {
261     return tanCordic(RAD, phi);
262 }
263
264 LUALIB_API void recr(fixed r, fixed theta, fixed *px, fixed *py) {
265     recCordic(RAD, r, theta, px, py);
266 }
267
268 LUALIB_API fixed asinr(fixed x) {
269     return asinCordic(RAD, x);
270 }
271
272 LUALIB_API fixed acosr(fixed x) {
273     return acosCordic(RAD, x);
274 }
275
276 LUALIB_API fixed atanr(fixed x) {
277     return atanCordic(RAD, x);
278 }
279
280 LUALIB_API void polr(fixed px, fixed py, fixed *r, fixed *theta) {
281     polCordic(RAD, px, py, r, theta);
282 }
283
284 //additional math functions
285 LUALIB_API fixed fint(fixed a) {
286     return cordic_sign(a) * (cordic_abs(a) & CORDIC_INTEGER);
287 }
288
289 LUALIB_API fixed fceil(fixed a) {
290     fixed int_a = fint(a);
291     return (a < 0 || a == int_a)? int_a: int_a + CORDIC_SCALE;
292 }
293
294 LUALIB_API fixed ffloor(fixed a) {
295     fixed int_a = fint(a);
296     return (a > 0 || a == int_a)? int_a: int_a - CORDIC_SCALE;
297 }
298
299 LUALIB_API fixed fround(fixed a) {
300     return ffloor(a + CORDIC_SCALE / 2);
301 }
302
303 // convert functions
304 LUALIB_API fixed floatToFixed(double a) {
305     int sign = 1;
306     if (a < 0) {
307         sign = -1;
308         a = -a;
309     }
310     if (a > FIXED_MAX) {
311         // error fixed overflow
312         a = FIXED_MAX;
313     }
314     return a * CORDIC_SCALE * sign;
315 }
316
317 LUALIB_API fixed intToFixed(int4b a, int round) {
318     double res = cordic_abs(a);
319     if (round) res = res + 0.5;
320     res = res * CORDIC_SCALE / INT_SCALE;
321     if (res > INT_MAX) {
322         // error fixed overflow
323         res = INT_MAX;
324     }
325     return cordic_sign(a) * res;
326 }
327
328 LUALIB_API int4b fixedToInt(fixed a, int round) {
329     double res = cordic_abs(a);
330     if (res > INT_MAX) {
331         // error int4b overflow
332         res = INT_MAX;
333     }
334     res = res * INT_SCALE / CORDIC_SCALE;
335     if (round) res = res + 0.5;
336     return cordic_sign(a) * res;
337 }
```

/* [<][>][^][v][top][bottom][index][help] */