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 "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] */