root/modules/cordic_math.c

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

DEFINITIONS

This source file includes following definitions.
  1. cordic
  2. cordic_abs
  3. cordic_sign
  4. muldivScaled
  5. mulScaled
  6. divScaled
  7. convertToQ1
  8. convertFromQ1
  9. rotateToQ1
  10. rotateFromQ1
  11. cathetus
  12. sincosCordic
  13. atanhypCordic
  14. sinCordic
  15. cosCordic
  16. tanCordic
  17. recCordic
  18. asinCordic
  19. acosCordic
  20. atanCordic
  21. polCordic
  22. sind
  23. cosd
  24. tand
  25. recd
  26. asind
  27. acosd
  28. atand
  29. pold
  30. sinr
  31. cosr
  32. tanr
  33. recr
  34. asinr
  35. acosr
  36. atanr
  37. polr
  38. fint
  39. fceil
  40. ffloor
  41. fround
  42. floatToFixed
  43. intToFixed
  44. 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 */
  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};   // {0.60725, 0.60724}
  22 fixed FULL_CIRCLE[]     = {0xc90fd, 0x2d00000}; // {6.28318, 360.00000}
  23 fixed HALF_CIRCLE[]     = {0x6487e, 0x1680000}; // {3.14159, 180.00000}
  24 fixed QUART_CIRCLE[]    = {0x3243f, 0xb40000};  // {1.57079, 90.00000}
  25 fixed ATAN_LIMIT        = 0x36f602be;           // 7035.00536
  26 fixed FIXED_MAX         = 16383.99999;
  27 
  28 // CORDIC main routine
  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 // abs(INT_MIN) doesn't work
  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 // a x b / c
  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             // error overflow
  65             res = INT_MAX;
  66         } else { res = sign * res; }
  67     } else {
  68         // error div 0
  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 // base CIRCULAR mode, ROTATE
 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 // base CIRCULAR mode, VECTOR
 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         // error input vales
 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         //shift to [pi; -pi]
 156         if ((q == 3) || (q == 4)) { z = z - FULL_CIRCLE[t]; }
 157         *phi = z;
 158         *hyp = (f)? 0 : x;
 159     }
 160 }
 161 
 162 // functions in CIRCULAR mode, ROTATE
 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 // functions in CIRCULAR mode, VECTOR
 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 // extern functions
 217 // DEG
 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 // RAD
 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 //additional math functions
 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 // convert functions
 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         // error fixed overflow
 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         // error fixed overflow
 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         // error int4b overflow
 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 }
/* [<][>][^][v][top][bottom][index][help] */