root/modules/cordic_math.c

DEFINITIONS

1. cordic
2. cordic_abs
3. cordic_sign
4. muldivScaled
5. mulScaled
6. divScaled
7. cathetus
8. sincosCordic
9. atanhypCordic
10. sinCordic
11. cosCordic
12. tanCordic
13. recCordic
14. asinCordic
15. acosCordic
16. atanCordic
17. polCordic
18. sind
19. cosd
20. tand
21. recd
22. asind
23. acosd
24. atand
25. pold
26. sinr
27. cosr
28. tanr
29. recr
30. asinr
31. acosr
32. atanr
33. polr
34. fint
35. fceil
36. ffloor
37. fround
38. floatToFixed
39. intToFixed
40. fixedToInt

   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 */
  12 #include "stdlib.h"
  13 #include "luaconf.h"
  14 #include "math.h"
  15 #include "cordic_math.h"
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 };
fixed INV_GAIN_CIRCLE[] = {0x136e9, 0x136e8};   // {0.60725, 0.60724}
fixed FULL_CIRCLE[]     = {0xc90fd, 0x2d00000}; // {6.28318, 360.00000}
fixed HALF_CIRCLE[]     = {0x6487e, 0x1680000}; // {3.14159, 180.00000}
fixed QUART_CIRCLE[]    = {0x3243f, 0xb40000};  // {1.57079, 90.00000}
fixed ATAN_LIMIT        = 0x36f602be;           // 7035.00536
fixed FIXED_MAX         = 16383.99999;
  28 // CORDIC main routine
  29 static 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) {
xstep = *x >> i;
ystep = *y >> i;
  36         if (i < M) {
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 }
  47 // abs(INT_MIN) doesn't work
  48 static int cordic_abs(int a) {
  49     if (a < -INT_MAX) a = -INT_MAX;
  50     return abs(a);
  51 }
  53 static int cordic_sign(int a) {
  54     return (a < 0)? -1 : 1;
  55 }
  57 // a x b / c
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);
res = FLOAT(cordic_abs(a)) * FLOAT(cordic_abs(b)) / FLOAT(cordic_abs(c));
  63         if (res > INT_MAX) {
  64             // error overflow
res = INT_MAX;
  66         } else { res = sign * res; }
  67     } else {
  68         // error div 0
res = 0;
  70     }
  71     return FIXED(res);
  72 }
  74 static fixed mulScaled(fixed a, fixed b) {
  75     return muldivScaled(a, b, CORDIC_SCALE);
  76 }
  78 static fixed divScaled(fixed a, fixed b) {
  79     return muldivScaled(CORDIC_SCALE, a, b);
  80 }
  82 static fixed cathetus(fixed x) {
  83     return FIXED(sqrt((1 + FLOAT(x)) * (1 - FLOAT(x))));
  84 }
  86 // base CIRCULAR mode, ROTATE
  87 static void sincosCordic(tangle t, fixed phi, fixed *sinphi, fixed *cosphi) {
  88     // convert to quadrant 1
phi = (phi % FULL_CIRCLE[t] + FULL_CIRCLE[t]) % FULL_CIRCLE[t];
  90     int q = phi / QUART_CIRCLE[t] + 1;
  91     switch(q) {
  92         case 2: phi = HALF_CIRCLE[t] - phi; break;
  93         case 3: phi = phi - HALF_CIRCLE[t]; break;
  94         case 4: phi = FULL_CIRCLE[t] - phi; break;
  95     }
fixed x = INV_GAIN_CIRCLE[t], y = 0, z = phi;
cordic(t, ROTATE, &x, &y, &z);
  98     // convert to original quadrant
  99     if ((q == 2) || (q == 3)) { x = -x; }
 100     if ((q == 3) || (q == 4)) { y = -y; }
 101     *sinphi = y;
 102     *cosphi = x;
 103 }
 105 // base CIRCULAR mode, VECTOR
 106 static void atanhypCordic(tangle t, fixed px, fixed py, fixed *phi, fixed *hyp) {
 107     // convert to quadrant 1
 108     int fy = (py >= 0);
 109     int q = 1;
 110     if (px < 0) {
 111         if (fy)
q = 2;
 113         else
q = 3;
 115     } else if (!fy) {
q = 4;
 117     }
px = cordic_abs(px);
py = cordic_abs(py);
 121     int f = 0;
 122     while (px > ATAN_LIMIT || py > ATAN_LIMIT) {
px /= 2;
py /= 2;
f = 1;
 126     }
 127     if (px == 0 && py == 0) {
 128         // error input vales
 129         *phi = 0;
 130         *hyp = 0;
 131     } else {
fixed x = px, y = py, z = 0;
cordic(t, VECTOR, &x, &y, &z);
 134         // convert to original quadrant
 135         switch(q) {
 136             case 2: z = HALF_CIRCLE[t] - z; break;
 137             case 3: z = z - HALF_CIRCLE[t]; break;
 138             case 4: z = -z; break;
 139         }
 140         *phi = z;
 141         *hyp = (f)? 0 : x;
 142     }
 143 }
 145 // functions in CIRCULAR mode, ROTATE
 146 static fixed sinCordic(tangle t, fixed phi) {
fixed _sin, _cos;
sincosCordic(t, phi, &_sin, &_cos);
 149     return _sin;
 150 }
 152 static fixed cosCordic(tangle t, fixed phi) {
fixed _sin, _cos;
sincosCordic(t, phi, &_sin, &_cos);
 155     return _cos;
 156 }
 158 static fixed tanCordic(tangle t, fixed phi) {
fixed _sin, _cos;
sincosCordic(t, phi, &_sin, &_cos);
 161     return divScaled(_sin, _cos);
 162 }
 164 static void recCordic(tangle t, fixed r, fixed theta, fixed *px, fixed *py) {
fixed _sin, _cos;
sincosCordic(t, theta, &_sin, &_cos);
 167     *px = mulScaled(r, _cos);
 168     *py = mulScaled(r, _sin);
 169 }
 171 // functions in CIRCULAR mode, VECTOR
 172 static fixed asinCordic(tangle t, fixed x) {
fixed phi, hyp;
fixed _cos = cathetus(x);
atanhypCordic(t, _cos, x, &phi, &hyp);
 176     return phi;
 177 }
 179 static fixed acosCordic(tangle t, fixed x) {
fixed phi, hyp;
fixed _sin = cathetus(x);
atanhypCordic(t, x, _sin, &phi, &hyp);
 183     return phi;
 184 }
 186 static fixed atanCordic(tangle t, fixed x) {
fixed phi, hyp;
atanhypCordic(t, CORDIC_SCALE, x, &phi, &hyp);
 189     return phi;
 190 }
 192 static void polCordic(tangle t, fixed px, fixed py, fixed *r, fixed *theta) {
fixed phi, hyp;
atanhypCordic(t, px, py, &phi, &hyp);
 195     *theta = phi;
 196     *r = mulScaled(hyp, INV_GAIN_CIRCLE[t]);
 197 }
 199 // extern functions
 200 // DEG
LUALIB_API fixed sind(fixed phi) {
 202     return sinCordic(DEG, phi);
 203 }
LUALIB_API fixed cosd(fixed phi) {
 206     return cosCordic(DEG, phi);
 207 }
LUALIB_API fixed tand(fixed phi) {
 210     return tanCordic(DEG, phi);
 211 }
LUALIB_API void recd(fixed r, fixed theta, fixed *px, fixed *py) {
recCordic(DEG, r, theta, px, py);
 215 }
LUALIB_API fixed asind(fixed x) {
 218     return asinCordic(DEG, x);
 219 }
LUALIB_API fixed acosd(fixed x) {
 222     return acosCordic(DEG, x);
 223 }
LUALIB_API fixed atand(fixed x) {
 226     return atanCordic(DEG, x);
 227 }
LUALIB_API void pold(fixed px, fixed py, fixed *r, fixed *theta) {
polCordic(DEG, px, py, r, theta);
 231 }
 233 // RAD
LUALIB_API fixed sinr(fixed phi) {
 235     return sinCordic(RAD, phi);
 236 }
LUALIB_API fixed cosr(fixed phi) {
 239     return cosCordic(RAD, phi);
 240 }
LUALIB_API fixed tanr(fixed phi) {
 243     return tanCordic(RAD, phi);
 244 }
LUALIB_API void recr(fixed r, fixed theta, fixed *px, fixed *py) {
recCordic(RAD, r, theta, px, py);
 248 }
LUALIB_API fixed asinr(fixed x) {
 251     return asinCordic(RAD, x);
 252 }
LUALIB_API fixed acosr(fixed x) {
 255     return acosCordic(RAD, x);
 256 }
LUALIB_API fixed atanr(fixed x) {
 259     return atanCordic(RAD, x);
 260 }
LUALIB_API void polr(fixed px, fixed py, fixed *r, fixed *theta) {
polCordic(RAD, px, py, r, theta);
 264 }
 266 //additional math functions
LUALIB_API fixed fint(fixed a) {
 268     return cordic_sign(a) * (cordic_abs(a) & CORDIC_INTEGER);
 269 }
LUALIB_API fixed fceil(fixed a) {
fixed int_a = fint(a);
 273     return (a < 0 || a == int_a)? int_a: int_a + CORDIC_SCALE;
 274 }
LUALIB_API fixed ffloor(fixed a) {
fixed int_a = fint(a);
 278     return (a > 0 || a == int_a)? int_a: int_a - CORDIC_SCALE;
 279 }
LUALIB_API fixed fround(fixed a) {
 282     return ffloor(a + CORDIC_SCALE / 2);
 283 }
 285 // convert functions
LUALIB_API fixed floatToFixed(double a) {
 287     int sign = 1;
 288     if (a < 0) {
sign = -1;
a = -a;
 291     }
 292     if (a > FIXED_MAX) {
 293         // error fixed overflow
a = FIXED_MAX;
 295     }
 296     return a * CORDIC_SCALE * sign;
 297 }
LUALIB_API fixed intToFixed(int4b a, int round) {
 300     double res = cordic_abs(a);
 301     if (round) res = res + 0.5;
res = res * CORDIC_SCALE / INT_SCALE;
 303     if (res > INT_MAX) {
 304         // error fixed overflow
res = INT_MAX;
 306     }
 307     return cordic_sign(a) * res;
 308 }
LUALIB_API int4b fixedToInt(fixed a, int round) {
 311     double res = cordic_abs(a);
 312     if (res > INT_MAX) {
 313         // error int4b overflow
res = INT_MAX;
 315     }
res = res * INT_SCALE / CORDIC_SCALE;
 317     if (round) res = res + 0.5;
 318     return cordic_sign(a) * res;
 319 }