/*
* CORDIC algorithms.
* The original code was published in Docter Dobbs Journal issue ddj9010.
* The ddj version can be obtained using FTP from SIMTEL and other places.
*
* Converted to ANSI-C (with prototypes) by P. Knoppers, 13-Apr-1992.
*
* The main advantage of the CORDIC algorithms is that all commonly used math
* functions ([a]sin[h] [a]cos[h] [a]tan[h] atah[h](y/x) ln exp sqrt) are
* implemented using only shift, add, subtract and compare. All values are
* treated as integers. Actually they are fixed point floating point values.
* The position of the fixed point is a compile time constant (fractionBits).
* I don't believe that this can be easily fixed...
*
* Some initialization of internal tables and constants is necessary before
* all functions can be used. The constant "HalfPi" must be determined before
* compile time, all others are computed during run-time - see main() below.
*
* Of course, any serious implementation of these functions should probably
* have _all_ constants determined sometime before run-time and most
* functions might be written in assembler.
*
*
* The layout of the code is adapted to my personal preferences.
*
* PK.
*/
/* Prototypes: (these should be moved to a separate include file) */
void WriteVarious (long n);
void WriteFraction (long n);
long Reciprocal (unsigned n, unsigned k);
long Poly2 (int log, unsigned n);
void Circular (long x, long y, long z);
void WriteRegisters (void);
long ScaledReciprocal (long n, unsigned k);
void InvertCircular (long x, long y, long z);
void Hyperbolic (long x, long y, long z);
void Linear (long x, long y, long z);
void InvertHyperbolic (long x, long y, long z);
/*
* _IMPLEMENTING CORDIC ALGORITHMS_
* by Pitts Jarvis
*
*
* [LISTING ONE]
*/
/*
* cordicC.c -- J. Pitts Jarvis, III
*
* cordicC.c computes CORDIC constants and exercises the basic algorithms.
* Represents all numbers in fixed point notation. 1 bit sign,
* longBits-1-n bit integral part, and n bit fractional part. n=29 lets us
* represent numbers in the interval [-4, 4) in 32 bit long. Two's
* complement arithmetic is operative here.
*/
#define fractionBits 29
#define longBits 32
#define One (010000000000L>>1)
#define HalfPi (014441766521L>>1)
/*
* cordic algorithm identities for circular functions, starting with [x, y, z]
* and then
* driving z to 0 gives: [P*(x*cos(z)-y*sin(z)), P*(y*cos(z)+x*sin(z)), 0]
* driving y to 0 gives: [P*sqrt(x^2+y^2), 0, z+atan(y/x)]
* where K = 1/P = sqrt(1+1)* . . . *sqrt(1+(2^(-2*i)))
* special cases which compute interesting functions
* sin, cos [K, 0, a] -> [cos(a), sin(a), 0]
* atan [1, a, 0] -> [sqrt(1+a^2)/K, 0, atan(a)]
* [x, y, 0] -> [sqrt(x^2+y^2)/K, 0, atan(y/x)]
* for hyperbolic functions, starting with [x, y, z] and then
* driving z to 0 gives: [P*(x*cosh(z)+y*sinh(z)), P*(y*cosh(z)+x*sinh(z)), 0]
* driving y to 0 gives: [P*sqrt(x^2-y^2), 0, z+atanh(y/x)]
* where K = 1/P = sqrt(1-(1/2)^2)* . . . *sqrt(1-(2^(-2*i)))
* sinh, cosh [K, 0, a] -> [cosh(a), sinh(a), 0]
* exponential [K, K, a] -> [e^a, e^a, 0]
* atanh [1, a, 0] -> [sqrt(1-a^2)/K, 0, atanh(a)]
* [x, y, 0] -> [sqrt(x^2-y^2)/K, 0, atanh(y/x)]
* ln [a+1, a-1, 0] -> [2*sqrt(a)/K, 0, ln(a)/2]
* sqrt [a+(K/2)^2, a-(K/2)^2, 0] -> [sqrt(a), 0, ln(a*(2/K)^2)/2]
* sqrt, ln [a+(K/2)^2, a-(K/2)^2, -ln(K/2)] -> [sqrt(a), 0, ln(a)/2]
* for linear functions, starting with [x, y, z] and then
* driving z to 0 gives: [x, y+x*z, 0]
* driving y to 0 gives: [x, 0, z+y/x]
*/
long X0C, X0H, X0R; /* seed for circular, hyperbolic, and square
* root */
long OneOverE, E; /* the base of natural logarithms */
long HalfLnX0R; /* constant used in simultanous sqrt, ln
* computation */
/*
* compute atan(x) and atanh(x) using infinite series
* atan(x) = x - x^3/3 + x^5/5 - x^7/7 + . . . for x^2 < 1
* atanh(x) = x + x^3/3 + x^5/5 + x^7/7 + . . . for x^2 < 1
* To calcuate these functions to 32 bits of precision, pick
* terms[i] s.t. ((2^-i)^(terms[i]))/(terms[i]) < 2^-32
* For x <= 2^(-11), atan(x) = atanh(x) = x with 32 bits of accuracy
*/
static unsigned terms[11] = {0, 27, 14, 9, 7, 5, 4, 4, 3, 3, 3};
static long a[28];
static long atan[fractionBits + 1];
static long atanh[fractionBits + 1];
static long X;
static long Y;
static long Z;
#include < stdio.h > /* putchar is a marco for some */
/* Delta is inefficient but pedagogical */
#define Delta(n, Z) (Z >= 0) ? (n) : -(n)
#define abs(n) (n >= 0) ? (n) : -(n)
/*
* Reciprocal, calculate reciprocol of n to k bits of precision
* a and r form integer and fractional parts of the dividend respectively
*/
long Reciprocal (unsigned n, unsigned k)
{
unsigned i;
unsigned a = 1;
long r = 0;
for (i = 0; i <= k; ++i)
{
r += r;
if (a >= n)
{
r += 1;
a -= n;
};
a += a;
}
return (a >= n ? r + 1 : r);/* round result */
}
/* ScaledReciprocal, n comes in funny fixed point fraction representation */
long ScaledReciprocal (long n, unsigned k)
{
long a;
long r = 0L;
unsigned i;
a = 1L << k;
for (i = 0; i <= k; ++i)
{
r += r;
if (a >= n)
{
r += 1;
a -= n;
};
a += a;
};
return (a >= n ? r + 1 : r);/* round result */
}
/*
* Poly2 calculates polynomial where the variable is an integral power of 2,
* log is the power of 2 of the variable
* n is the order of the polynomial
* coefficients are in the array a[]
*/
long Poly2 (int log, unsigned n)
{
long r = 0;
int i;
for (i = n; i >= 0; --i)
r = (log < 0 ? r >> -log : r << log) + a[i];
return (r);
}
void WriteFraction (long n)
{
unsigned short i;
unsigned short low;
unsigned short digit;
unsigned long k;
putchar (n < 0 ? '-' : ' ');
n = abs (n);
putchar ((n >> fractionBits) + '0');
putchar ('.');
low = k = n << (longBits - fractionBits); /* align octal point at left */
k >>= 4; /* shift to make room for a decimal digit */
for (i = 1; i <= 8; ++i)
{
digit = (k *= 10L) >> (longBits - 4);
low = (low & 0xf) * 10;
k += ((unsigned long) (low >> 4)) -
((unsigned long) digit << (longBits - 4));
putchar (digit + '0');
}
}
void WriteRegisters (void)
{
printf (" X: ");
WriteVarious (X);
printf (" Y: ");
WriteVarious (Y);
printf (" Z: ");
WriteVarious (Z);
}
void WriteVarious (long n)
{
WriteFraction (n);
printf (" 0x%08lx 0%011lo\n", n, n);
}
void Circular (long x, long y, long z)
{
int i;
X = x;
Y = y;
Z = z;
for (i = 0; i <= fractionBits; ++i)
{
x = X >> i;
y = Y >> i;
z = atan[i];
X -= Delta (y, Z);
Y += Delta (x, Z);
Z -= Delta (z, Z);
}
}
void InvertCircular (long x, long y, long z)
{
int i;
X = x;
Y = y;
Z = z;
for (i = 0; i <= fractionBits; ++i)
{
x = X >> i;
y = Y >> i;
z = atan[i];
X -= Delta (y, -Y);
Z -= Delta (z, -Y);
Y += Delta (x, -Y);
}
}
void Hyperbolic (long x, long y, long z)
{
int i;
X = x;
Y = y;
Z = z;
for (i = 1; i <= fractionBits; ++i)
{
x = X >> i;
y = Y >> i;
z = atanh[i];
X += Delta (y, Z);
Y += Delta (x, Z);
Z -= Delta (z, Z);
if ((i == 4) || (i == 13))
{
x = X >> i;
y = Y >> i;
z = atanh[i];
X += Delta (y, Z);
Y += Delta (x, Z);
Z -= Delta (z, Z);
}
}
}
void InvertHyperbolic (long x, long y, long z)
{
int i;
X = x;
Y = y;
Z = z;
for (i = 1; i <= fractionBits; ++i)
{
x = X >> i;
y = Y >> i;
z = atanh[i];
X += Delta (y, -Y);
Z -= Delta (z, -Y);
Y += Delta (x, -Y);
if ((i == 4) || (i == 13))
{
x = X >> i;
y = Y >> i;
z = atanh[i];
X += Delta (y, -Y);
Z -= Delta (z, -Y);
Y += Delta (x, -Y);
}
}
}
void Linear (long x, long y, long z)
{
int i;
X = x;
Y = y;
Z = z;
z = One;
for (i = 1; i <= fractionBits; ++i)
{
x >>= 1;
z >>= 1;
Y += Delta (x, Z);
Z -= Delta (z, Z);
}
}
void InvertLinear (long x, long y, long z)
{
int i;
X = x;
Y = y;
Z = z;
z = One;
for (i = 1; i <= fractionBits; ++i)
{
Z -= Delta (z >>= 1, -Y);
Y += Delta (x >>= 1, -Y);
}
}
/*********************************************************/
int main (int argc, char *argv[])
{
int i;
long r;
/* system("date"); *//* time stamp the log for UNIX systems */
for (i = 0; i <= 13; ++i)
{
a[2 * i] = 0;
a[2 * i + 1] = Reciprocal (2 * i + 1, fractionBits);
}
for (i = 0; i <= 10; ++i)
atanh[i] = Poly2 (-i, terms[i]);
atan[0] = HalfPi / 2; /* atan(2^0)= pi / 4 */
for (i = 1; i <= 7; ++i)
a[4 * i - 1] = -a[4 * i - 1];
for (i = 1; i <= 10; ++i)
atan[i] = Poly2 (-i, terms[i]);
for (i = 11; i <= fractionBits; ++i)
atan[i] = atanh[i] = 1L << (fractionBits - i);
printf ("\natanh(2^-n)\n");
for (i = 1; i <= 10; ++i)
{
printf ("%2d ", i);
WriteVarious (atanh[i]);
}
r = 0;
for (i = 1; i <= fractionBits; ++i)
r += atanh[i];
r += atanh[4] + atanh[13];
printf ("radius of convergence");
WriteFraction (r);
printf ("\n\natan(2^-n)\n");
for (i = 0; i <= 10; ++i)
{
printf ("%2d ", i);
WriteVarious (atan[i]);
}
r = 0;
for (i = 0; i <= fractionBits; ++i)
r += atan[i];
printf ("radius of convergence");
WriteFraction (r);
/* all the results reported in the printfs are calculated with my HP-41C */
printf ("\n\n--------------------circular functions--------------------\n");
printf ("Grinding on [1, 0, 0]\n");
Circular (One, 0L, 0L);
WriteRegisters ();
printf ("\n K: ");
WriteVarious (X0C = ScaledReciprocal (X, fractionBits));
printf ("\nGrinding on [K, 0, 0]\n");
Circular (X0C, 0L, 0L);
WriteRegisters ();
printf ("\nGrinding on [K, 0, pi/6] -> [0.86602540, 0.50000000, 0]\n");
Circular (X0C, 0L, HalfPi / 3L);
WriteRegisters ();
printf ("\nGrinding on [K, 0, pi/4] -> [0.70710678, 0.70710678, 0]\n");
Circular (X0C, 0L, HalfPi / 2L);
WriteRegisters ();
printf ("\nGrinding on [K, 0, pi/3] -> [0.50000000, 0.86602540, 0]\n");
Circular (X0C, 0L, 2L * (HalfPi / 3L));
WriteRegisters ();
printf ("\n------Inverse functions------\n");
printf ("Grinding on [1, 0, 0]\n");
InvertCircular (One, 0L, 0L);
WriteRegisters ();
printf ("\nGrinding on [1, 1/2, 0] -> [1.84113394, 0, 0.46364761]\n");
InvertCircular (One, One / 2L, 0L);
WriteRegisters ();
printf ("\nGrinding on [2, 1, 0] -> [3.68226788, 0, 0.46364761]\n");
InvertCircular (One * 2L, One, 0L);
WriteRegisters ();
printf ("\nGrinding on [1, 5/8, 0] -> [1.94193815, 0, 0.55859932]\n");
InvertCircular (One, 5L * (One / 8L), 0L);
WriteRegisters ();
printf ("\nGrinding on [1, 1, 0] -> [2.32887069, 0, 0.78539816]\n");
InvertCircular (One, One, 0L);
WriteRegisters ();
printf ("\n--------------------hyperbolic functions--------------------\n");
printf ("Grinding on [1, 0, 0]\n");
Hyperbolic (One, 0L, 0L);
WriteRegisters ();
printf ("\n K: ");
WriteVarious (X0H = ScaledReciprocal (X, fractionBits));
printf (" R: ");
X0R = X0H >> 1;
Linear (X0R, 0L, X0R);
WriteVarious (X0R = Y);
printf ("\nGrinding on [K, 0, 0]\n");
Hyperbolic (X0H, 0L, 0L);
WriteRegisters ();
printf ("\nGrinding on [K, 0, 1] -> [1.54308064, 1.17520119, 0]\n");
Hyperbolic (X0H, 0L, One);
WriteRegisters ();
printf ("\nGrinding on [K, K, -1] -> [0.36787944, 0.36787944, 0]\n");
Hyperbolic (X0H, X0H, -One);
WriteRegisters ();
OneOverE = X; /* save value ln(1/e) = -1 */
printf ("\nGrinding on [K, K, 1] -> [2.71828183, 2.71828183, 0]\n");
Hyperbolic (X0H, X0H, One);
WriteRegisters ();
E = X; /* save value ln(e) = 1 */
printf ("\n------Inverse functions------\n");
printf ("Grinding on [1, 0, 0]\n");
InvertHyperbolic (One, 0L, 0L);
WriteRegisters ();
printf ("\nGrinding on [1/e + 1, 1/e - 1, 0] -> [1.00460806, 0, -0.50000000]\n");
InvertHyperbolic (OneOverE + One, OneOverE - One, 0L);
WriteRegisters ();
printf ("\nGrinding on [e + 1, e - 1, 0] -> [2.73080784, 0, 0.50000000]\n");
InvertHyperbolic (E + One, E - One, 0L);
WriteRegisters ();
printf ("\nGrinding on (1/2)*ln(3) -> [0.71720703, 0, 0.54930614]\n");
InvertHyperbolic (One, One / 2L, 0L);
WriteRegisters ();
printf ("\nGrinding on [3/2, -1/2, 0] -> [1.17119417, 0, -0.34657359]\n");
InvertHyperbolic (One + (One / 2L), -(One / 2L), 0L);
WriteRegisters ();
printf ("\nGrinding on sqrt(1/2) -> [0.70710678, 0, 0.15802389]\n");
InvertHyperbolic (One / 2L + X0R, One / 2L - X0R, 0L);
WriteRegisters ();
printf ("\nGrinding on sqrt(1) -> [1.00000000, 0, 0.50449748]\n");
InvertHyperbolic (One + X0R, One - X0R, 0L);
WriteRegisters ();
HalfLnX0R = Z;
printf ("\nGrinding on sqrt(2) -> [1.41421356, 0, 0.85117107]\n");
InvertHyperbolic (One * 2L + X0R, One * 2L - X0R, 0L);
WriteRegisters ();
printf ("\nGrinding on sqrt(1/2), ln(1/2)/2 -> [0.70710678, 0, -0.34657359]\n");
InvertHyperbolic (One / 2L + X0R, One / 2L - X0R, -HalfLnX0R);
WriteRegisters ();
printf ("\nGrinding on sqrt(3)/2, ln(3/4)/2 -> [0.86602540, 0, -0.14384104]\n");
InvertHyperbolic ((3L * One / 4L) + X0R, (3L * One / 4L) - X0R, -HalfLnX0R);
WriteRegisters ();
printf ("\nGrinding on sqrt(2), ln(2)/2 -> [1.41421356, 0, 0.34657359]\n");
InvertHyperbolic (One * 2L + X0R, One * 2L - X0R, -HalfLnX0R);
WriteRegisters ();
return (0);
}
/*
[LISTING TWO]
atanh (2^-n)
1 0.54930614 0x1193ea7a 002144765172
2 0.25541281 0x082c577d 001013053575
3 0.12565721 0x04056247 000401261107
4 0.06258157 0x0200ab11 000200125421
5 0.03126017 0x01001558 000100012530
6 0.01562627 0x008002aa 000040001252
7 0.00781265 0x00400055 000020000125
8 0.00390626 0x0020000a 000010000012
9 0.00195312 0x00100001 000004000001
10 0.00097656 0x00080000 000002000000
radius of convergence 1.11817300
atan (2^-n)
0 0.78539816 0x1921fb54 003110375524
1 0.46364760 0x0ed63382 001665431602
2 0.24497866 0x07d6dd7e 000765556576
3 0.12435499 0x03fab753 000376533523
4 0.06241880 0x01ff55bb 000177652673
5 0.03123983 0x00ffeaad 000077765255
6 0.01562372 0x007ffd55 000037776525
7 0.00781233 0x003fffaa 000017777652
8 0.00390622 0x001ffff5 000007777765
9 0.00195312 0x000ffffe 000003777776
10 0.00097656 0x0007ffff 000001777777
radius of convergence 1.74328660
*/
Comments from Peter Knoppers
Good idea setting up a page about Cordic algorithms!
Maybe you would like to add a page with the program that I have had
lying around for years now. It nicely demostrates Cordic algorithms.
Requires ANSI C-compiler.
Greetings from Delft, the Netherlands,
_____ __ __ __ __ ____ _____ _____ ______ _____ _____
| _ \ | |/ || \| | / \ | _ \ | _ \ | ___|| _ \ / ___|
| __/ _ | < | || || || __/ | __/ | >__ | < \__ \
|__| |_| |__|\__||__|\__| \____/ |__| |__| |______||__|\__||_____/
Delft University of Technology, The Netherlands. Phone +31-(0)15-2144886
Email: knop@duteca.et.tudelft.nl WWW: http://cardit.et.tudelft.nl/~knop/