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