home *** CD-ROM | disk | FTP | other *** search
/ Power-Programmierung / CD1.mdf / magazine / drdobbs / 1990 / 10 / jarvis.asc < prev    next >
Text File  |  1990-08-16  |  12KB  |  325 lines
  1. _IMPLEMENTING CORDIC ALGORITHMS_
  2. by Pitts Jarvis
  3.  
  4.  
  5.  [LISTING ONE]
  6.  
  7. /* cordicC.c -- J. Pitts Jarvis, III 
  8.  * cordicC.c computes CORDIC constants and exercises the basic algorithms.
  9.  * Represents all numbers in fixed point notation.  1 bit sign,
  10.  * longBits-1-n bit integral part, and n bit fractional part.  n=29 lets us
  11.  * represent numbers in the interval [-4, 4) in 32 bit long.  Two's
  12.  * complement arithmetic is operative here. 
  13.  */
  14.  
  15. #define fractionBits 29
  16. #define longBits 32
  17. #define One    (010000000000>>1)
  18. #define HalfPi (014441766521>>1)
  19.  
  20. /* cordic algorithm identities for circular functions, starting with [x, y, z] 
  21.  *  and then
  22.  *  driving z to 0 gives: [P*(x*cos(z)-y*sin(z)), P*(y*cos(z)+x*sin(z)), 0]
  23.  *  driving y to 0 gives: [P*sqrt(x^2+y^2), 0, z+atan(y/x)]
  24.  *  where K = 1/P = sqrt(1+1)* . . . *sqrt(1+(2^(-2*i)))
  25.  *  special cases which compute interesting functions
  26.  *  sin, cos      [K, 0, a] -> [cos(a), sin(a), 0]
  27.  *  atan          [1, a, 0] -> [sqrt(1+a^2)/K, 0, atan(a)]
  28.  *        [x, y, 0] -> [sqrt(x^2+y^2)/K, 0, atan(y/x)]
  29.  * for hyperbolic functions, starting with [x, y, z] and then
  30.  * driving z to 0 gives: [P*(x*cosh(z)+y*sinh(z)), P*(y*cosh(z)+x*sinh(z)), 0]
  31.  * driving y to 0 gives: [P*sqrt(x^2-y^2), 0, z+atanh(y/x)]
  32.  * where K = 1/P = sqrt(1-(1/2)^2)* . . . *sqrt(1-(2^(-2*i)))
  33.  *  sinh, cosh    [K, 0, a] -> [cosh(a), sinh(a), 0]
  34.  *  exponential   [K, K, a] -> [e^a, e^a, 0]
  35.  *  atanh         [1, a, 0] -> [sqrt(1-a^2)/K, 0, atanh(a)]
  36.  *           [x, y, 0] -> [sqrt(x^2-y^2)/K, 0, atanh(y/x)]
  37.  *  ln            [a+1, a-1, 0] -> [2*sqrt(a)/K, 0, ln(a)/2]
  38.  *  sqrt          [a+(K/2)^2, a-(K/2)^2, 0] -> [sqrt(a), 0, ln(a*(2/K)^2)/2]
  39.  *  sqrt, ln      [a+(K/2)^2, a-(K/2)^2, -ln(K/2)] -> [sqrt(a), 0, ln(a)/2]
  40.  * for linear functions, starting with [x, y, z] and then
  41.  *  driving z to 0 gives: [x, y+x*z, 0]
  42.  *  driving y to 0 gives: [x, 0, z+y/x] 
  43.  */
  44.  
  45. long X0C, X0H, X0R; /* seed for circular, hyperbolic, and square root */
  46. long OneOverE, E;   /* the base of natural logarithms */
  47. long HalfLnX0R;        /* constant used in simultanous sqrt, ln computation */
  48.  
  49. /* compute atan(x) and atanh(x) using infinite series
  50.  *    atan(x) =  x - x^3/3 + x^5/5 - x^7/7 + . . . for x^2 < 1
  51.  *     atanh(x) = x + x^3/3 + x^5/5 + x^7/7 + . . . for x^2 < 1
  52.  * To calcuate these functions to 32 bits of precision, pick
  53.  * terms[i] s.t. ((2^-i)^(terms[i]))/(terms[i]) < 2^-32
  54.  * For x <= 2^(-11), atan(x) = atanh(x) = x with 32 bits of accuracy  */
  55. unsigned terms[11]= {0, 27, 14, 9, 7, 5, 4, 4, 3, 3, 3};èstatic long a[28], atan[fractionBits+1], atanh[fractionBits+1], X, Y, Z;
  56. #include <stdio.h>    /* putchar is a marco for some */
  57.  
  58. /* Delta is inefficient but pedagogical */
  59. #define Delta(n, Z) (Z>=0) ? (n) : -(n)
  60. #define abs(n) (n>=0) ? (n) : -(n)
  61.  
  62. /* Reciprocal, calculate reciprocol of n to k bits of precision
  63.  *  a and r form integer and fractional parts of the dividend respectively  */
  64. long
  65. Reciprocal(n, k) unsigned n, k; 
  66. {
  67.   unsigned i, a= 1; long r= 0;
  68.   for (i= 0; i<=k; ++i) {r += r; if (a>=n) {r += 1; a -= n;}; a += a;}
  69.   return(a>=n? r+1 : r); /* round result */ 
  70. }
  71.  
  72. /* ScaledReciprocal, n comes in funny fixed point fraction representation */
  73. long
  74. ScaledReciprocal(n, k) long n; unsigned k; 
  75. {
  76.   long a, r=0; unsigned i;
  77.   a= 1L<<k;
  78.   for (i=0; i<=k; ++i) {r += r; if (a>=n) {r += 1; a -= n;}; a += a;};
  79.   return(a>=n? r+1 : r); /* round result */ 
  80. }
  81.  
  82. /* Poly2 calculates polynomial where the variable is an integral power of 2,
  83.  *    log is the power of 2 of the variable
  84.  *    n is the order of the polynomial
  85.  *    coefficients are in the array a[]   */
  86. long
  87. Poly2(log, n) int log; unsigned n; 
  88. {
  89.   long r=0; int i;
  90.   for (i=n; i>=0; --i) r= (log<0? r>>-log : r<<log)+a[i];
  91.   return(r);
  92. }
  93. WriteFraction(n) long n;
  94. {
  95.   unsigned short i, low, digit; unsigned long k;
  96.   putchar(n < 0 ? '-' : ' '); n = abs(n);
  97.   putchar((n>>fractionBits) + '0'); putchar('.');
  98.   low = k = n << (longBits-fractionBits); /* align octal point at left */
  99.   k   >>=  4;    /* shift to make room for a decimal digit */
  100.   for (i=1; i<=8; ++i) 
  101.   {
  102.     digit = (k *= 10L) >> (longBits-4);
  103.     low = (low & 0xf) * 10; 
  104.     k += ((unsigned long) (low>>4)) - ((unsigned long) digit << (longBits-4));
  105.     putchar(digit+'0');
  106.   }
  107. }
  108. WriteRegisters()
  109. {è  printf("  X: "); WriteVarious(X);
  110.   printf("  Y: "); WriteVarious(Y);
  111.   printf("  Z: "); WriteVarious(Z);
  112. }
  113. WriteVarious(n) long n;
  114. {
  115.   WriteFraction(n); printf("  0x%08lx 0%011lo\n", n, n);
  116. }
  117. Circular(x, y, z) long x, y, z;
  118. {
  119.   int i;
  120.   X = x; Y = y; Z = z;
  121.   for (i=0; i<=fractionBits; ++i)
  122.   {
  123.     x= X>>i; y= Y>>i; z= atan[i];
  124.     X -= Delta(y, Z);
  125.     Y += Delta(x, Z);
  126.     Z -= Delta(z, Z);
  127.   }
  128. }
  129. InvertCircular(x, y, z) long x, y, z;
  130. {
  131.   int i;
  132.   X = x; Y = y; Z = z;
  133.   for (i=0; i<=fractionBits; ++i)
  134.   {
  135.     x= X>>i; y= Y>>i; z= atan[i];
  136.     X -= Delta(y, -Y);
  137.     Z -= Delta(z, -Y);
  138.     Y += Delta(x, -Y);
  139.   }
  140. }
  141. Hyperbolic(x, y, z) long x, y, z;
  142. {
  143.   int i;
  144.   X = x; Y = y; Z = z;
  145.   for (i=1; i<=fractionBits; ++i)
  146.   {
  147.     x= X>>i; y= Y>>i; z= atanh[i];
  148.     X += Delta(y, Z);
  149.     Y += Delta(x, Z);
  150.     Z -= Delta(z, Z);
  151.     if ((i==4)||(i==13))
  152.     {
  153.       x= X>>i; y= Y>>i; z= atanh[i];
  154.       X  +=  Delta(y, Z);
  155.       Y += Delta(x, Z);
  156.       Z  -=  Delta(z, Z);
  157.     }
  158.   }
  159. }
  160. InvertHyperbolic(x, y, z) long x, y, z;
  161. {
  162.   int i;
  163.   X = x; Y = y; Z = z;è  for (i=1; i<=fractionBits; ++i)
  164.   {
  165.     x= X>>i; y= Y>>i; z= atanh[i];
  166.     X += Delta(y, -Y);
  167.     Z -= Delta(z, -Y);
  168.     Y += Delta(x, -Y);
  169.     if ((i==4)||(i==13))
  170.     {
  171.       x= X>>i; y= Y>>i; z= atanh[i];
  172.       X += Delta(y, -Y);
  173.       Z -= Delta(z, -Y);
  174.       Y += Delta(x, -Y);
  175.     }
  176.   }
  177. }
  178. Linear(x, y, z) long x, y, z;
  179. {
  180.   int i;
  181.   X = x; Y = y; Z = z; z= One;
  182.   for (i=1; i<=fractionBits; ++i)
  183.   {
  184.     x  >>= 1; z  >>= 1; Y += Delta(x, Z); Z -= Delta(z, Z);
  185.   }
  186. }
  187. InvertLinear(x, y, z) long x, y, z;
  188. {
  189.   int i;
  190.   X = x; Y = y; Z = z; z= One;
  191.   for (i=1; i<=fractionBits; ++i) 
  192.   {
  193.      Z -= Delta(z  >>= 1, -Y); Y += Delta(x  >>= 1, -Y);
  194.   }
  195. }
  196.  
  197. /*********************************************************/
  198. main()
  199. {
  200.   int i; long r;
  201.   /*system("date");*//* time stamp the log for UNIX systems */
  202.   for (i=0; i<=13; ++i)
  203.   {
  204.     a[2*i]= 0; a[2*i+1]= Reciprocal(2*i+1, fractionBits);
  205.   }
  206.   for (i=0; i<=10; ++i) atanh[i]= Poly2(-i, terms[i]);
  207.   atan[0]= HalfPi/2; /* atan(2^0)= pi/4 */
  208.   for (i=1; i<=7; ++i) a[4*i-1]= -a[4*i-1];
  209.   for (i=1; i<=10; ++i) atan[i]= Poly2(-i, terms[i]);
  210.   for (i=11; i<=fractionBits; ++i) atan[i]= atanh[i]= 1L<<(fractionBits-i);
  211.   printf("\natanh(2^-n)\n");
  212.   for (i=1; i<=10; ++i){printf("%2d ", i); WriteVarious(atanh[i]);}
  213.   r= 0; 
  214.   for (i=1; i<=fractionBits; ++i) 
  215.      r +=  atanh[i]; 
  216.   r +=  atanh[4]+atanh[13];
  217.   printf("radius of convergence"); WriteFraction(r);è  printf("\n\natan(2^-n)\n");
  218.   for (i=0; i<=10; ++i){printf("%2d ", i); WriteVarious(atan[i]);}
  219.   r= 0; for (i=0; i<=fractionBits; ++i) r +=  atan[i];
  220.   printf("radius of convergence"); WriteFraction(r);
  221.  
  222.   /* all the results reported in the printfs are calculated with my HP-41C */
  223.   printf("\n\n--------------------circular functions--------------------\n");
  224.     printf("Grinding on [1, 0, 0]\n");
  225.     Circular(One, 0L, 0L); WriteRegisters();
  226.     printf("\n  K: "); WriteVarious(X0C= ScaledReciprocal(X, fractionBits));
  227.     printf("\nGrinding on [K, 0, 0]\n");
  228.     Circular(X0C, 0L, 0L); WriteRegisters();
  229.     printf("\nGrinding on [K, 0, pi/6] -> [0.86602540, 0.50000000, 0]\n");
  230.     Circular(X0C, 0L, HalfPi/3L); WriteRegisters();
  231.     printf("\nGrinding on [K, 0, pi/4] -> [0.70710678, 0.70710678, 0]\n");
  232.     Circular(X0C, 0L, HalfPi/2L); WriteRegisters();
  233.     printf("\nGrinding on [K, 0, pi/3] -> [0.50000000, 0.86602540, 0]\n");
  234.     Circular(X0C, 0L, 2L*(HalfPi/3L)); WriteRegisters();
  235.   printf("\n------Inverse functions------\n");
  236.     printf("Grinding on [1, 0, 0]\n");
  237.     InvertCircular(One, 0L, 0L); WriteRegisters();
  238.     printf("\nGrinding on [1, 1/2, 0] -> [1.84113394, 0, 0.46364761]\n");
  239.     InvertCircular(One, One/2L, 0L); WriteRegisters();
  240.     printf("\nGrinding on [2, 1, 0] -> [3.68226788, 0, 0.46364761]\n");
  241.     InvertCircular(One*2L, One, 0L); WriteRegisters();
  242.     printf("\nGrinding on [1, 5/8, 0] -> [1.94193815, 0, 0.55859932]\n");
  243.     InvertCircular(One, 5L*(One/8L), 0L); WriteRegisters();
  244.     printf("\nGrinding on [1, 1, 0] -> [2.32887069, 0, 0.78539816]\n");
  245.     InvertCircular(One, One, 0L); WriteRegisters();
  246.   printf("\n--------------------hyperbolic functions--------------------\n");
  247.     printf("Grinding on [1, 0, 0]\n");
  248.     Hyperbolic(One, 0L, 0L); WriteRegisters();
  249.     printf("\n  K: "); WriteVarious(X0H= ScaledReciprocal(X, fractionBits));
  250.     printf("  R: "); X0R= X0H>>1; Linear(X0R, 0L, X0R); WriteVarious(X0R= Y);
  251.     printf("\nGrinding on [K, 0, 0]\n");
  252.     Hyperbolic(X0H, 0L, 0L); WriteRegisters();
  253.     printf("\nGrinding on [K, 0, 1] -> [1.54308064, 1.17520119, 0]\n");
  254.     Hyperbolic(X0H, 0L, One); WriteRegisters();
  255.     printf("\nGrinding on [K, K, -1] -> [0.36787944, 0.36787944, 0]\n");
  256.     Hyperbolic(X0H, X0H, -One); WriteRegisters();
  257.     OneOverE = X; /* save value ln(1/e) = -1 */
  258.     printf("\nGrinding on [K, K, 1] -> [2.71828183, 2.71828183, 0]\n");
  259.     Hyperbolic(X0H, X0H, One); WriteRegisters();
  260.     E = X; /* save value ln(e) = 1 */
  261.   printf("\n------Inverse functions------\n");
  262.     printf("Grinding on [1, 0, 0]\n");
  263.     InvertHyperbolic(One, 0L, 0L); WriteRegisters();
  264.     printf("\nGrinding on [1/e + 1, 1/e - 1, 0] -> [1.00460806, 0, 
  265.                                                              -0.50000000]\n");
  266.     InvertHyperbolic(OneOverE+One,OneOverE-One, 0L); WriteRegisters();
  267.     printf("\nGrinding on [e + 1, e - 1, 0] -> [2.73080784, 0, 0.50000000]\n");
  268.     InvertHyperbolic(E+One, E-One, 0L); WriteRegisters();
  269.     printf("\nGrinding on (1/2)*ln(3) -> [0.71720703, 0, 0.54930614]\n");
  270.     InvertHyperbolic(One, One/2L, 0L); WriteRegisters();
  271.     printf("\nGrinding on [3/2, -1/2, 0] -> [1.17119417, 0, -0.34657359]\n");è    InvertHyperbolic(One+(One/2L), -(One/2L), 0L); WriteRegisters();
  272.     printf("\nGrinding on sqrt(1/2) -> [0.70710678, 0, 0.15802389]\n");
  273.     InvertHyperbolic(One/2L+X0R, One/2L-X0R, 0L); WriteRegisters();
  274.     printf("\nGrinding on sqrt(1) -> [1.00000000, 0, 0.50449748]\n");
  275.     InvertHyperbolic(One+X0R, One-X0R, 0L); WriteRegisters();
  276.     HalfLnX0R = Z;
  277.     printf("\nGrinding on sqrt(2) -> [1.41421356, 0, 0.85117107]\n");
  278.     InvertHyperbolic(One*2L+X0R, One*2L-X0R, 0L); WriteRegisters();
  279.     printf("\nGrinding on sqrt(1/2), ln(1/2)/2 -> [0.70710678, 0, 
  280.                                                               -0.34657359]\n");
  281.     InvertHyperbolic(One/2L+X0R, One/2L-X0R, -HalfLnX0R); WriteRegisters();
  282.     printf("\nGrinding on sqrt(3)/2, ln(3/4)/2 -> [0.86602540, 0, 
  283.                                                               -0.14384104]\n");
  284.     InvertHyperbolic((3L*One/4L)+X0R, (3L*One/4L)-X0R, -HalfLnX0R);
  285.     WriteRegisters();
  286.     printf("\nGrinding on sqrt(2), ln(2)/2 -> [1.41421356, 0, 0.34657359]\n");
  287.     InvertHyperbolic(One*2L+X0R, One*2L-X0R, -HalfLnX0R);
  288.     WriteRegisters();
  289.     exit(0);
  290. }
  291.  
  292.  
  293.  
  294. [LISTING TWO]
  295.  
  296. atanh (2^-n)
  297.  
  298. 1   0.54930614   0x1193ea7a 002144765172
  299. 2   0.25541281   0x082c577d 001013053575
  300. 3   0.12565721   0x04056247 000401261107
  301. 4   0.06258157   0x0200ab11 000200125421
  302. 5   0.03126017   0x01001558 000100012530
  303. 6   0.01562627   0x008002aa 000040001252
  304. 7   0.00781265   0x00400055 000020000125
  305. 8   0.00390626   0x0020000a 000010000012
  306. 9   0.00195312   0x00100001 000004000001
  307. 10  0.00097656   0x00080000 000002000000
  308.  
  309. radius of convergence 1.11817300
  310.  
  311. atan (26-n)
  312.  
  313. 0   0.78539816   0x1921fb54 003110375524
  314. 1   0.46364760   0x0ed63382 001665431602
  315. 2   0.24497866   0x07d6dd7e 000765556576
  316. 3   0.12435499   0x03fab753 000376533523
  317. 4   0.06241880   0x01ff55bb 000177652673
  318. 5   0.03123983   0x00ffeaad 000077765255
  319. 6   0.01562372   0x007ffd55 000037776525
  320. 7   0.00781233   0x003fffaa 000017777652
  321. 8   0.00390622   0x001ffff5 000007777765
  322. 9   0.00195312   0x000ffffe 000003777776
  323. 10  0.00097656   0x0007ffff 000001777777
  324.  
  325. radius of convergence 1.74328660è