cordic.c

   1 /*
   2  * CORDIC algorithms.
   3  * The original code was published in Docter Dobbs Journal issue ddj9010.
   4  * The ddj version can be obtained using FTP from SIMTEL and other places.
   5  *
   6  * Converted to ANSI-C (with prototypes) by P. Knoppers, 13-Apr-1992.
   7  *
   8  * The main advantage of the CORDIC algorithms is that all commonly used math
   9  * functions ([a]sin[h] [a]cos[h] [a]tan[h] atah[h](y/x) ln exp sqrt) are
  10  * implemented using only shift, add, subtract and compare. All values are
  11  * treated as integers. Actually they are fixed point floating point values.
  12  * The position of the fixed point is a compile time constant (fractionBits).
  13  * I don't believe that this can be easily fixed...
  14  *
  15  * Some initialization of internal tables and constants is necessary before
  16  * all functions can be used. The constant "HalfPi" must be determined before
  17  * compile time, all others are computed during run-time - see main() below.
  18  *
  19  * Of course, any serious implementation of these functions should probably
  20  * have _all_ constants determined sometime before run-time and most
  21  * functions might be written in assembler.
  22  *
  23  *
  24  * The layout of the code is adapted to my personal preferences.
  25  *
  26  * PK.
  27  */
  28 /* Prototypes: (these should be moved to a separate include file) */
  29
  30 void WriteVarious (long n);
  31 void WriteFraction (long n);
  32 long Reciprocal (unsigned n, unsigned k);
  33 long Poly2 (int log, unsigned n);
  34 void Circular (long x, long y, long z);
  35 void WriteRegisters (void);
  36 long ScaledReciprocal (long n, unsigned k);
  37 void InvertCircular (long x, long y, long z);
  38 void Hyperbolic (long x, long y, long z);
  39 void Linear (long x, long y, long z);
  40 void InvertHyperbolic (long x, long y, long z);
  41
  42 /*
  43  * _IMPLEMENTING CORDIC ALGORITHMS_
  44  * by Pitts Jarvis
  45  *
  46  *
  47  * [LISTING ONE]
  48  */
  49
  50 /*
  51  * cordicC.c -- J. Pitts Jarvis, III
  52  *
  53  * cordicC.c computes CORDIC constants and exercises the basic algorithms.
  54  * Represents all numbers in fixed point notation.  1 bit sign,
  55  * longBits-1-n bit integral part, and n bit fractional part.  n=29 lets us
  56  * represent numbers in the interval [-4, 4) in 32 bit long.  Two's
  57  * complement arithmetic is operative here.
  58  */
  59
  60 #define fractionBits    29
  61 #define longBits        32
  62 #define One             (010000000000L>>1)
  63 #define HalfPi          (014441766521L>>1)
  64
  65 /*
  66  * cordic algorithm identities for circular functions, starting with [x, y, z]
  67  * and then
  68  * driving z to 0 gives: [P*(x*cos(z)-y*sin(z)), P*(y*cos(z)+x*sin(z)), 0]
  69  * driving y to 0 gives: [P*sqrt(x^2+y^2), 0, z+atan(y/x)]
  70  * where K = 1/P = sqrt(1+1)* . . . *sqrt(1+(2^(-2*i)))
  71  * special cases which compute interesting functions
  72  *  sin, cos      [K, 0, a] -> [cos(a), sin(a), 0]
  73  *  atan          [1, a, 0] -> [sqrt(1+a^2)/K, 0, atan(a)]
  74  *                [x, y, 0] -> [sqrt(x^2+y^2)/K, 0, atan(y/x)]
  75  * for hyperbolic functions, starting with [x, y, z] and then
  76  * driving z to 0 gives: [P*(x*cosh(z)+y*sinh(z)), P*(y*cosh(z)+x*sinh(z)), 0]
  77  * driving y to 0 gives: [P*sqrt(x^2-y^2), 0, z+atanh(y/x)]
  78  * where K = 1/P = sqrt(1-(1/2)^2)* . . . *sqrt(1-(2^(-2*i)))
  79  *  sinh, cosh    [K, 0, a] -> [cosh(a), sinh(a), 0]
  80  *  exponential   [K, K, a] -> [e^a, e^a, 0]
  81  *  atanh         [1, a, 0] -> [sqrt(1-a^2)/K, 0, atanh(a)]
  82  *                [x, y, 0] -> [sqrt(x^2-y^2)/K, 0, atanh(y/x)]
  83  *  ln            [a+1, a-1, 0] -> [2*sqrt(a)/K, 0, ln(a)/2]
  84  *  sqrt          [a+(K/2)^2, a-(K/2)^2, 0] -> [sqrt(a), 0, ln(a*(2/K)^2)/2]
  85  *  sqrt, ln      [a+(K/2)^2, a-(K/2)^2, -ln(K/2)] -> [sqrt(a), 0, ln(a)/2]
  86  * for linear functions, starting with [x, y, z] and then
  87  *  driving z to 0 gives: [x, y+x*z, 0]
  88  *  driving y to 0 gives: [x, 0, z+y/x]
  89  */
  90
  91 long X0C, X0H, X0R;             /* seed for circular, hyperbolic, and square
  92                                  * root */
  93 long OneOverE, E;               /* the base of natural logarithms */
  94 long HalfLnX0R;                 /* constant used in simultanous sqrt, ln
  95                                  * computation */
  96
  97 /*
  98  * compute atan(x) and atanh(x) using infinite series
  99  *  atan(x) =  x - x^3/3 + x^5/5 - x^7/7 + . . . for x^2 < 1
 100  *  atanh(x) = x + x^3/3 + x^5/5 + x^7/7 + . . . for x^2 < 1
 101  * To calcuate these functions to 32 bits of precision, pick
 102  * terms[i] s.t. ((2^-i)^(terms[i]))/(terms[i]) < 2^-32
 103  * For x <= 2^(-11), atan(x) = atanh(x) = x with 32 bits of accuracy
 104  */
 105
 106 static unsigned terms[11] = {0, 27, 14, 9, 7, 5, 4, 4, 3, 3, 3};
 107 static long a[28];
 108 static long atan[fractionBits + 1];
 109 static long atanh[fractionBits + 1];
 110 static long X;
 111 static long Y;
 112 static long Z;
 113
 114 #include <stdio.h>              /* putchar is a marco for some */
 115
 116 /* Delta is inefficient but pedagogical */
 117 #define Delta(n, Z)     (Z >= 0) ? (n) : -(n)
 118 #define abs(n)          (n >= 0) ? (n) : -(n)
 119
 120 /*
 121  * Reciprocal, calculate reciprocol of n to k bits of precision
 122  * a and r form integer and fractional parts of the dividend respectively
 123  */
 124 long Reciprocal (unsigned n, unsigned k)
 125 {
 126     unsigned i;
 127     unsigned a = 1;
 128     long r = 0;
 129
 130     for (i = 0; i <= k; ++i)
 131     {
 132         r += r;
 133         if (a >= n)
 134         {
 135             r += 1;
 136             a -= n;
 137         };
 138         a += a;
 139     }
 140     return (a >= n ? r + 1 : r);/* round result */
 141 }
 142
 143 /* ScaledReciprocal, n comes in funny fixed point fraction representation */
 144 long ScaledReciprocal (long n, unsigned k)
 145 {
 146     long a;
 147     long r = 0L;
 148     unsigned i;
 149
 150     a = 1L << k;
 151     for (i = 0; i <= k; ++i)
 152     {
 153         r += r;
 154         if (a >= n)
 155         {
 156             r += 1;
 157             a -= n;
 158         };
 159         a += a;
 160     };
 161     return (a >= n ? r + 1 : r);/* round result */
 162 }
 163
 164 /*
 165  * Poly2 calculates polynomial where the variable is an integral power of 2,
 166  *      log is the power of 2 of the variable
 167  *      n is the order of the polynomial
 168  *      coefficients are in the array a[]
 169  */
 170 long Poly2 (int log, unsigned n)
 171 {
 172     long r = 0;
 173     int i;
 174
 175     for (i = n; i >= 0; --i)
 176         r = (log < 0 ? r >> -log : r << log) + a[i];
 177     return (r);
 178 }
 179
 180 void WriteFraction (long n)
 181 {
 182     unsigned short i;
 183     unsigned short low;
 184     unsigned short digit;
 185     unsigned long k;
 186
 187     putchar (n < 0 ? '-' : ' ');
 188     n = abs (n);
 189     putchar ((n >> fractionBits) + '0');
 190     putchar ('.');
 191     low = k = n << (longBits - fractionBits);   /* align octal point at left */
 192     k >>= 4;                    /* shift to make room for a decimal digit */
 193     for (i = 1; i <= 8; ++i)
 194     {
 195         digit = (k *= 10L) >> (longBits - 4);
 196         low = (low & 0xf) * 10;
 197         k += ((unsigned long) (low >> 4)) -
 198                 ((unsigned long) digit << (longBits - 4));
 199         putchar (digit + '0');
 200     }
 201 }
 202
 203 void WriteRegisters (void)
 204 {
 205     printf ("  X: ");
 206     WriteVarious (X);
 207     printf ("  Y: ");
 208     WriteVarious (Y);
 209     printf ("  Z: ");
 210     WriteVarious (Z);
 211 }
 212
 213 void WriteVarious (long n)
 214 {
 215     WriteFraction (n);
 216     printf ("  0x%08lx 0%011lo\n", n, n);
 217 }
 218
 219 void Circular (long x, long y, long z)
 220 {
 221     int i;
 222
 223     X = x;
 224     Y = y;
 225     Z = z;
 226
 227     for (i = 0; i <= fractionBits; ++i)
 228     {
 229         x = X >> i;
 230         y = Y >> i;
 231         z = atan[i];
 232         X -= Delta (y, Z);
 233         Y += Delta (x, Z);
 234         Z -= Delta (z, Z);
 235     }
 236 }
 237
 238 void InvertCircular (long x, long y, long z)
 239 {
 240     int i;
 241
 242     X = x;
 243     Y = y;
 244     Z = z;
 245
 246     for (i = 0; i <= fractionBits; ++i)
 247     {
 248         x = X >> i;
 249         y = Y >> i;
 250         z = atan[i];
 251         X -= Delta (y, -Y);
 252         Z -= Delta (z, -Y);
 253         Y += Delta (x, -Y);
 254     }
 255 }
 256
 257 void Hyperbolic (long x, long y, long z)
 258 {
 259     int i;
 260
 261     X = x;
 262     Y = y;
 263     Z = z;
 264
 265     for (i = 1; i <= fractionBits; ++i)
 266     {
 267         x = X >> i;
 268         y = Y >> i;
 269         z = atanh[i];
 270         X += Delta (y, Z);
 271         Y += Delta (x, Z);
 272         Z -= Delta (z, Z);
 273         if ((i == 4) || (i == 13))
 274         {
 275             x = X >> i;
 276             y = Y >> i;
 277             z = atanh[i];
 278             X += Delta (y, Z);
 279             Y += Delta (x, Z);
 280             Z -= Delta (z, Z);
 281         }
 282     }
 283 }
 284
 285 void InvertHyperbolic (long x, long y, long z)
 286 {
 287     int i;
 288
 289     X = x;
 290     Y = y;
 291     Z = z;
 292     for (i = 1; i <= fractionBits; ++i)
 293     {
 294         x = X >> i;
 295         y = Y >> i;
 296         z = atanh[i];
 297         X += Delta (y, -Y);
 298         Z -= Delta (z, -Y);
 299         Y += Delta (x, -Y);
 300         if ((i == 4) || (i == 13))
 301         {
 302             x = X >> i;
 303             y = Y >> i;
 304             z = atanh[i];
 305             X += Delta (y, -Y);
 306             Z -= Delta (z, -Y);
 307             Y += Delta (x, -Y);
 308         }
 309     }
 310 }
 311
 312 void Linear (long x, long y, long z)
 313 {
 314     int i;
 315
 316     X = x;
 317     Y = y;
 318     Z = z;
 319     z = One;
 320
 321     for (i = 1; i <= fractionBits; ++i)
 322     {
 323         x >>= 1;
 324         z >>= 1;
 325         Y += Delta (x, Z);
 326         Z -= Delta (z, Z);
 327     }
 328 }
 329
 330 void InvertLinear (long x, long y, long z)
 331 {
 332     int i;
 333
 334     X = x;
 335     Y = y;
 336     Z = z;
 337     z = One;
 338
 339     for (i = 1; i <= fractionBits; ++i)
 340     {
 341         Z -= Delta (z >>= 1, -Y);
 342         Y += Delta (x >>= 1, -Y);
 343     }
 344 }
 345
 346 /*********************************************************/
 347
 348 int main (int argc, char *argv[])
 349 {
 350     int i;
 351     long r;
 352
 353     /* system("date"); *//* time stamp the log for UNIX systems */
 354
 355     for (i = 0; i <= 13; ++i)
 356     {
 357         a[2 * i] = 0;
 358         a[2 * i + 1] = Reciprocal (2 * i + 1, fractionBits);
 359     }
 360     for (i = 0; i <= 10; ++i)
 361         atanh[i] = Poly2 (-i, terms[i]);
 362     atan[0] = HalfPi / 2;       /* atan(2^0)= pi / 4 */
 363     for (i = 1; i <= 7; ++i)
 364         a[4 * i - 1] = -a[4 * i - 1];
 365     for (i = 1; i <= 10; ++i)
 366         atan[i] = Poly2 (-i, terms[i]);
 367     for (i = 11; i <= fractionBits; ++i)
 368         atan[i] = atanh[i] = 1L << (fractionBits - i);
 369
 370     printf ("\natanh(2^-n)\n");
 371     for (i = 1; i <= 10; ++i)
 372     {
 373         printf ("%2d ", i);
 374         WriteVarious (atanh[i]);
 375     }
 376
 377     r = 0;
 378     for (i = 1; i <= fractionBits; ++i)
 379         r += atanh[i];
 380     r += atanh[4] + atanh[13];
 381     printf ("radius of convergence");
 382     WriteFraction (r);
 383     printf ("\n\natan(2^-n)\n");
 384     for (i = 0; i <= 10; ++i)
 385     {
 386         printf ("%2d ", i);
 387         WriteVarious (atan[i]);
 388     }
 389
 390     r = 0;
 391     for (i = 0; i <= fractionBits; ++i)
 392         r += atan[i];
 393     printf ("radius of convergence");
 394     WriteFraction (r);
 395
 396     /* all the results reported in the printfs are calculated with my HP-41C */
 397     printf ("\n\n--------------------circular functions--------------------\n");
 398
 399     printf ("Grinding on [1, 0, 0]\n");
 400     Circular (One, 0L, 0L);
 401     WriteRegisters ();
 402     printf ("\n  K: ");
 403     WriteVarious (X0C = ScaledReciprocal (X, fractionBits));
 404
 405     printf ("\nGrinding on [K, 0, 0]\n");
 406     Circular (X0C, 0L, 0L);
 407     WriteRegisters ();
 408
 409     printf ("\nGrinding on [K, 0, pi/6] -> [0.86602540, 0.50000000, 0]\n");
 410     Circular (X0C, 0L, HalfPi / 3L);
 411     WriteRegisters ();
 412
 413     printf ("\nGrinding on [K, 0, pi/4] -> [0.70710678, 0.70710678, 0]\n");
 414     Circular (X0C, 0L, HalfPi / 2L);
 415     WriteRegisters ();
 416
 417     printf ("\nGrinding on [K, 0, pi/3] -> [0.50000000, 0.86602540, 0]\n");
 418     Circular (X0C, 0L, 2L * (HalfPi / 3L));
 419     WriteRegisters ();
 420
 421     printf ("\n------Inverse functions------\n");
 422
 423     printf ("Grinding on [1, 0, 0]\n");
 424     InvertCircular (One, 0L, 0L);
 425     WriteRegisters ();
 426
 427     printf ("\nGrinding on [1, 1/2, 0] -> [1.84113394, 0, 0.46364761]\n");
 428     InvertCircular (One, One / 2L, 0L);
 429     WriteRegisters ();
 430
 431     printf ("\nGrinding on [2, 1, 0] -> [3.68226788, 0, 0.46364761]\n");
 432     InvertCircular (One * 2L, One, 0L);
 433     WriteRegisters ();
 434
 435     printf ("\nGrinding on [1, 5/8, 0] -> [1.94193815, 0, 0.55859932]\n");
 436     InvertCircular (One, 5L * (One / 8L), 0L);
 437     WriteRegisters ();
 438
 439     printf ("\nGrinding on [1, 1, 0] -> [2.32887069, 0, 0.78539816]\n");
 440     InvertCircular (One, One, 0L);
 441     WriteRegisters ();
 442
 443     printf ("\n--------------------hyperbolic functions--------------------\n");
 444
 445     printf ("Grinding on [1, 0, 0]\n");
 446     Hyperbolic (One, 0L, 0L);
 447     WriteRegisters ();
 448     printf ("\n  K: ");
 449     WriteVarious (X0H = ScaledReciprocal (X, fractionBits));
 450     printf ("  R: ");
 451     X0R = X0H >> 1;
 452     Linear (X0R, 0L, X0R);
 453     WriteVarious (X0R = Y);
 454
 455     printf ("\nGrinding on [K, 0, 0]\n");
 456     Hyperbolic (X0H, 0L, 0L);
 457     WriteRegisters ();
 458
 459     printf ("\nGrinding on [K, 0, 1] -> [1.54308064, 1.17520119, 0]\n");
 460     Hyperbolic (X0H, 0L, One);
 461     WriteRegisters ();
 462
 463     printf ("\nGrinding on [K, K, -1] -> [0.36787944, 0.36787944, 0]\n");
 464     Hyperbolic (X0H, X0H, -One);
 465     WriteRegisters ();
 466     OneOverE = X;               /* save value ln(1/e) = -1 */
 467
 468     printf ("\nGrinding on [K, K, 1] -> [2.71828183, 2.71828183, 0]\n");
 469     Hyperbolic (X0H, X0H, One);
 470     WriteRegisters ();
 471     E = X;                      /* save value ln(e) = 1 */
 472
 473     printf ("\n------Inverse functions------\n");
 474
 475     printf ("Grinding on [1, 0, 0]\n");
 476     InvertHyperbolic (One, 0L, 0L);
 477     WriteRegisters ();
 478
 479     printf ("\nGrinding on [1/e + 1, 1/e - 1, 0] -> [1.00460806, 0, -0.50000000]\n");
 480     InvertHyperbolic (OneOverE + One, OneOverE - One, 0L);
 481     WriteRegisters ();
 482
 483     printf ("\nGrinding on [e + 1, e - 1, 0] -> [2.73080784, 0, 0.50000000]\n");
 484     InvertHyperbolic (E + One, E - One, 0L);
 485     WriteRegisters ();
 486
 487     printf ("\nGrinding on (1/2)*ln(3) -> [0.71720703, 0, 0.54930614]\n");
 488     InvertHyperbolic (One, One / 2L, 0L);
 489     WriteRegisters ();
 490
 491     printf ("\nGrinding on [3/2, -1/2, 0] -> [1.17119417, 0, -0.34657359]\n");
 492     InvertHyperbolic (One + (One / 2L), -(One / 2L), 0L);
 493     WriteRegisters ();
 494
 495     printf ("\nGrinding on sqrt(1/2) -> [0.70710678, 0, 0.15802389]\n");
 496     InvertHyperbolic (One / 2L + X0R, One / 2L - X0R, 0L);
 497     WriteRegisters ();
 498
 499     printf ("\nGrinding on sqrt(1) -> [1.00000000, 0, 0.50449748]\n");
 500     InvertHyperbolic (One + X0R, One - X0R, 0L);
 501     WriteRegisters ();
 502     HalfLnX0R = Z;
 503
 504     printf ("\nGrinding on sqrt(2) -> [1.41421356, 0, 0.85117107]\n");
 505     InvertHyperbolic (One * 2L + X0R, One * 2L - X0R, 0L);
 506     WriteRegisters ();
 507
 508     printf ("\nGrinding on sqrt(1/2), ln(1/2)/2 -> [0.70710678, 0, -0.34657359]\n");
 509     InvertHyperbolic (One / 2L + X0R, One / 2L - X0R, -HalfLnX0R);
 510     WriteRegisters ();
 511
 512     printf ("\nGrinding on sqrt(3)/2, ln(3/4)/2 -> [0.86602540, 0, -0.14384104]\n");
 513     InvertHyperbolic ((3L * One / 4L) + X0R, (3L * One / 4L) - X0R, -HalfLnX0R);
 514     WriteRegisters ();
 515
 516     printf ("\nGrinding on sqrt(2), ln(2)/2 -> [1.41421356, 0, 0.34657359]\n");
 517     InvertHyperbolic (One * 2L + X0R, One * 2L - X0R, -HalfLnX0R);
 518     WriteRegisters ();
 519     return (0);
 520 }
 521
 522
 523 /*
 524 [LISTING TWO]
 525
 526 atanh (2^-n)
 527
 528 1   0.54930614   0x1193ea7a 002144765172
 529 2   0.25541281   0x082c577d 001013053575
 530 3   0.12565721   0x04056247 000401261107
 531 4   0.06258157   0x0200ab11 000200125421
 532 5   0.03126017   0x01001558 000100012530
 533 6   0.01562627   0x008002aa 000040001252
 534 7   0.00781265   0x00400055 000020000125
 535 8   0.00390626   0x0020000a 000010000012
 536 9   0.00195312   0x00100001 000004000001
 537 10  0.00097656   0x00080000 000002000000
 538
 539 radius of convergence 1.11817300
 540
 541 atan (2^-n)
 542
 543 0   0.78539816   0x1921fb54 003110375524
 544 1   0.46364760   0x0ed63382 001665431602
 545 2   0.24497866   0x07d6dd7e 000765556576
 546 3   0.12435499   0x03fab753 000376533523
 547 4   0.06241880   0x01ff55bb 000177652673
 548 5   0.03123983   0x00ffeaad 000077765255
 549 6   0.01562372   0x007ffd55 000037776525
 550 7   0.00781233   0x003fffaa 000017777652
 551 8   0.00390622   0x001ffff5 000007777765
 552 9   0.00195312   0x000ffffe 000003777776
 553 10  0.00097656   0x0007ffff 000001777777
 554
 555 radius of convergence 1.74328660
 556 */