#include <perms.h>
// Edinburgh IMP77 Compiler - Version 8.4
//
// 1 ! This is effectively a fixed-point implementation of CORDIC sin and cos which
// 2 ! checks results against the floating-point library used by C. There are a few
// 3 ! things we could do to improve accuracy - but this is good enough for a quick
// 4 ! hack that does not use any external math library code, other than the afore-
// 5 ! mentioned consistency checks.
// 6
// 7 ! The program is based on Michael Bertrand's CORDIC article which has appeared
// 8 ! in multiple places including Dr Dobbs magazine, a copy of which can be found
// 9 ! at http://www.pennelynn.com/Documents/CUJ/HTML/92HTML/1992024C.HTM
// 10
// 11 ! Note that in creating this implementation, we hit a bug that manifested at
// 12 ! the the extremes of the sin and cos functions (where the values are close
// 13 ! to 0 or 1) and which I have patched with an approximation since the functions
// 14 ! are close to linear at those points. However the hack to correct that error
// 15 ! does make this code a little larger than the C version from which it was
// 16 ! derived.
// 17
// 18 ! I have thrown in a local implementation of atan and sqrt for good measure :-)
// 19
// 20 ! Imp77 does not have unsigned integer data types; however since the CORDIC code
// 21 ! effectively uses a 1.14 fixed point implementation to implement angles (with
// 22 ! the original C sources being compatible with a 16-bit int environment), and
// 23 ! the current Imp77 compiler being a 32 bit implementation, unsigned integers
// 24 ! have not required.
// 25
// 26 %begin
int main(int argc, char **argv) { /*%begin*/
_imp_initialise(argc, argv);
// 27
// 28 %const %integer debug = 0 { 0 : 4 }
// 29
// 30 !%external %real %fn %spec atan %alias "atanf" (%real angle) ;! borrow C's math library for use in demo.
// 31 %real %fn atan(%longreal x1)
float ATAN(double X1) {
// 32 %const %long %real r1= 1.2595802263029547 @ 1
const double R1 = 1.2595802263029547E1;
// 33 %const %long %real r2=-8.6186317517509520 @ 1
const double R2 = -8.6186317517509520E1;
// 34 %const %long %real r3=-1.2766919133361079 @ 0
const double R3 = -1.2766919133361079;
// 35 %const %long %real r4=-8.3921038065840512 @ -2
const double R4 = -8.3921038065840512E-2;
// 36 %const %long %real s1= 2.7096164294378656 @ 1
const double S1 = 2.7096164294378656E1;
// 37 %const %long %real s2= 6.5581320451487386 @ 0
const double S2 = 6.5581320451487386;
// 38 %const %long %real s3= 2.1441643116703661 @ 0
const double S3 = 2.1441643116703661;
// 39 %const %long %real s4= 1.2676256708212610 @ 0
const double S4 = 1.2676256708212610;
// 40 %const %long %real rt3= 1.7320508075688772 @ 0
const double RT3 = 1.7320508075688772;
// 41 %const %long %real piby6= 5.2359877559829887 @ -1
const double PIBY6 = 5.2359877559829887E-1;
// 42 %const %long %real piby2m1= 5.7079632679489661 @ -1
const double PIBY2M1 = 5.7079632679489661E-1;
// 43 %const %long %real rt3m1=7.3205080756887728 @ -1
const double RT3M1 = 7.3205080756887728E-1;
// 44 %const %long %real tanpiby12= 2.6794919243112271 @ -1
const double TANPIBY12 = 2.6794919243112271E-1;
// 45 %long %real xx1,xsq,constant=0
double XX1;
double XSQ;
double CONSTANT;
CONSTANT = 0;
// 46 %integer sign=0,inv=0
int SIGN;
SIGN = 0;
int INV;
INV = 0;
// 47
// 48 %if x1<0 %then sign=1 %and xx1=-x1 %else xx1=x1
if (X1 >= 0) goto L_0001;
SIGN = 1;
XX1 = -X1;
goto L_0002;
L_0001:;
XX1 = X1;
L_0002:;
// 49 %if xx1>1.0 %then xx1=1.0/xx1 %and inv=1
if (XX1 <= 1.0) goto L_0003;
XX1 = (double)(1.0 / XX1);
INV = 1;
L_0003:;
// 50 %if xx1>tanpiby12 %then xx1=(rt3m1*xx1-1.0+xx1)/(xx1+rt3) %and constant=piby6
if (XX1 <= TANPIBY12) goto L_0004;
XX1 = (double)((RT3M1 * XX1 - 1.0 + XX1) / (XX1 + RT3));
CONSTANT = PIBY6;
L_0004:;
// 51 xsq=xx1*xx1
XSQ = XX1 * XX1;
// 52 xx1 = xx1*(r1/(xsq+s1+r2/(xsq+s2+r3/(xsq+s3+r4/(xsq+s4)))))
XX1 =
XX1 *
((double)(R1 / (XSQ + S1 + (double)(R2 / (XSQ + S2 + (double)(R3 / (XSQ + S3 + (double)(R4 / (XSQ + S4)))))))));
// 53 xx1 = xx1 + constant
XX1 = XX1 + CONSTANT;
// 54 %if inv=1 %then xx1=1.0-xx1+piby2m1
if (INV != 1) goto L_0005;
XX1 = 1.0 - XX1 + PIBY2M1;
L_0005:;
// 55 %if sign=1 %then xx1=-xx1
if (SIGN != 1) goto L_0006;
XX1 = -XX1;
L_0006:;
// 56 %result=xx1 { shorten from %long %real down to %real when returning result }
return XX1;
// 57 %end
} /* end of ATAN */
// 58
// 59 !%external %real %fn %spec sqrt %alias "sqrtf" (%real angle)
// 60 %real %fn sqrt(%long %real X)
float SQRT(double X) {
// 61 %integer %fn approx integer sqrt(%integer X)
int APPROXINTEGERSQRT(int X) {
// 62 %integer k = 1, result = 1
int K;
K = 1;
int RESULT;
RESULT = 1;
// 63 k = k+1 %and result = k*k %while result <= X
L_0007:;
if (RESULT > X) goto L_0008;
K = K + 1;
RESULT = K * K;
goto L_0007;
L_0008:;
// 64 %result = k-1 {note 'off by 1' bug in original C version due to returning k--}
return K - 1;
// 65 %end
} /* end of APPROXINTEGERSQRT */
// 66 %integer I
int I;
// 67 %long %real prev A, A, epsilon = X / 100 {accurate enough due to .14 fixed point}
double PREVA;
double A;
double EPSILON;
EPSILON = (double)(X / 100);
// 68 %if X = 2.0 %then %result = 1.41421356237 %else I = approx integer sqrt(INT PT(X))
if (X != 2.0) goto L_0009;
return 1.41421356237;
L_0009:;
I = APPROXINTEGERSQRT(_imp_INTPT(X));
// 69 %result = I %if X = I*I { exact root }
if (X != I * I) goto L_000A;
return I;
L_000A:;
// 70 ! Newton's method: A{n} = (A{n-1} + X/A{n-1})/2
// 71 A = I; prev A = A %and A = (A + X/A)/2.0 %until prev A - epsilon <= A %and A <= prev A + epsilon
A = I;
L_000B:;
PREVA = A;
A = (double)((A + (double)(X / A)) / 2.0);
if (PREVA - EPSILON > A) goto L_000C;
if (A <= PREVA + EPSILON) goto L_000D;
L_000C:;
goto L_000B;
L_000D:;
// 72 %result = A
return A;
// 73 %end
} /* end of SQRT */
// 74
// 75 %real PI = atan(1) * 4
float PI;
PI = ATAN(1) * 4;
// 76
// 77 %const %integer K64 = 1<<16, K32 = K64>>1, K16 = K32>>1, K8 = K16>>1
// 78
// 79 %routine phex(%integer x)
void PHEX(int X) {
// 80 {%const} %string (16) hex = "0123456789ABCDEF"
_imp_string16 HEX;
/* clang-format off */
_imp_strncpy(/*?*/&HEX, ({__typeof__(_imp_string255) tmp;/*B*/ tmp = _imp_str_literal("0123456789ABCDEF"); (void *)&tmp;}), /*max:*/16);
/* clang-format on */
// 81 %integer i
int I;
// 82 print symbol(charno(hex, (x >> (i<<2))&15+1)) %for i = 7, -1, 0
// %for I = 7, -1, 0 %cycle
{
I = 8;
L_000E:
if (I == 0) goto L_000F;
I -= 1;
_imp_PRINTSYMBOL(*_imp_CHARNO((_imp_string255 *)/*p*/ /*?*/ &HEX, ((unsigned int)X >> (I << 2) & 15) + 1));
goto L_000E;
}
L_000F:;
// 83 %end
return;
} /* end of PHEX */
// 84
// 85 %routine SinCos({unsigned} %integer theta, %integer %name sin, %integer %name cos)
void SINCOS(int THETA, int *SIN, int *COS) {
// 86 %const %integer QUAD1 = 3, QUAD2 = 2, QUAD3 = 0, QUAD4 = 1 { bit 1 for COS x vs -x, bit 2 for SIN x vs -x }
// 87 %const %integer NBITS = 14 { NBITS is number of bits for CORDIC units. }
// 88
// 89 {unsigned} %const %integer CordicBase = 1 << NBITS { base for CORDIC units }
// 90 {unsigned} %const %integer HalfBase = CordicBase >> 1 { CordicBase / 2 }
// 91 {unsigned} %const %integer Quad2Boundary = CordicBase << 1 { 2 * CordicBase }
// 92 {unsigned} %const %integer Quad3Boundary = CordicBase + Quad2Boundary { 3 * CordicBase }
// 93
// 94 ! these owns are initialised on the first call to SinCos:
// 95 %own %integer %array ArcTan(0:NBITS-1) { angles for rotation }
static int ARCTAN[14];
struct _imp_dv ARCTAN_dv = {ARCTAN, sizeof(int) /*, dims, <bounds> */};
// 96 %own %integer xInit { initial x projection }
static int XINIT;
// 97
// 98 %integer xcos,xsin { used to patch a problem that happens near the zeroes and extremes of the results }
int XCOS;
int XSIN;
// 99
// 100 ! USE : Calc sin and cos with integer CORDIC routine.
// 101 ! IN : theta = incoming angle (in CORDIC angle units)
// 102 ! OUT : sin (in CORDIC fixed point units)
// 103 ! cos (in CORDIC fixed point units)
// 104 ! NOTE: The incoming angle theta is in CORDIC angle
// 105 ! units, which subdivide the circle into 64K parts,
// 106 ! with 0 deg = 0, 90 deg = 16384 (CordicBase), 180 deg
// 107 ! = 32768, 270 deg = 49152, etc. The calculated sine
// 108 ! and cosine are in CORDIC fixed point units - an int
// 109 ! considered as a fraction of 16384 (CordicBase).
// 110
// 111 %integer quadrant { quadrant of incoming angle }
int QUADRANT;
// 112 %integer z { incoming angle moved to 1st quad }
int Z;
// 113 %integer i { to index rotations - one per bit }
int I;
// 114 %integer x, y { projections onto axes }
int X;
int Y;
// 115 %integer xl, yl { projections of rotated vector }
int XL;
int YL;
// 116
// 117 %own %integer initialised = 0
static int INITIALISED = 0;
// 118 %if initialised = 0 %start
if (INITIALISED != 0) goto L_0011;
// 119
// 120 ! USE : Load globals used by SinCos().
// 121 ! OUT : Loads globals used in SinCos() :
// 122 ! CordicBase = base for CORDIC units
// 123 ! HalfBase = Cordicbase / 2
// 124 ! Quad2Boundary = 2 * CordicBase
// 125 ! Quad3Boundary = 3 * CordicBase
// 126 ! ArcTan() = the arctans of 1/(2^i)
// 127 ! xInit = initial value for x projection
// 128 ! NOTE: Must be called once only to initialize before
// 129 ! calling SinCos(). xInit is sufficiently less than
// 130 ! CordicBase to exactly compensate for the expansion
// 131 ! in each rotation.
// 132
// 133 {%integer i} { to index ArcTan() }
// 134 %real f { to calc initial x projection }
float F;
// 135 %integer powr { powers of 2 up to 2^(2*(NBITS-1)) }
int POWR;
// 136
// 137 { ArcTans are diminishingly small angles. }
// 138 powr = 1
POWR = 1;
// 139 %for i = 0, 1, NBITS-1 %cycle
// %for I = 0, 1, 14 - 1 %cycle
{
int _final = 14 - 1;
if (invalid_for_loop(0, 1, _final)) _imp_signal(5, 1, 0, "Illegal cycle I = 0, 1, 14 - 1");
I = -1;
L_0012:
if (I == _final) goto L_0013;
I += 1;
// 140
// 141 ! We could alternatively have created ArcTan as an array of constants at compile time.
// 142 ArcTan(i) = INT(atan(1.0/powr)/(PI/2.0)*CordicBase)
ARCTAN[I] = _imp_INT((double)(ATAN((double)(1.0 / POWR)) / ((double)(PI / 2.0))) * 16384);
// 143 %if debug > 1 %start
if (0 <= 1) goto L_0015;
// 144 print string("Setting ArcTan("); write(i,0); print string(") to "); phex(ArcTan(i)); newline
_imp_PRINTSTRING(_imp_str_literal("Setting ArcTan("));
_imp_WRITE(I, 0);
_imp_PRINTSTRING(_imp_str_literal(") to "));
PHEX(ARCTAN[I]);
_imp_NEWLINE();
// 145 %finish
L_0015:;
// 146 powr = powr << 1
POWR = POWR << 1;
// 147 ! xInit is initial value of x projection to compensate for expansions. f = 1/sqrt(2/1 * 5/4 * ...
// 148 ! Normalize as an NBITS binary fraction (multiply by CordicBase) and store in xInit.
// 149 ! Get f = 0.607253 and xInit = 9949 = 0x26DD for NBITS = 14.
// 150 %repeat
goto L_0012;
}
L_0013:;
// 151 f = 1.0
F = 1.0;
// 152
// 153 powr = 1
POWR = 1;
// 154 %for i = 0, 1, NBITS-1 %cycle
// %for I = 0, 1, 14 - 1 %cycle
{
int _final = 14 - 1;
if (invalid_for_loop(0, 1, _final)) _imp_signal(5, 1, 0, "Illegal cycle I = 0, 1, 14 - 1");
I = -1;
L_0016:
if (I == _final) goto L_0017;
I += 1;
// 155 f = (f * (powr + 1)) / powr
F = (double)((F * (POWR + 1)) / POWR);
// 156 powr = powr << 2
POWR = POWR << 2;
// 157 %repeat
goto L_0016;
}
L_0017:;
// 158
// 159 f = 1.0/sqrt(f); { sqrt is expensive but only done once during initialisation }
F = (double)(1.0 / SQRT(F));
// 160 { Note could have implemented f = rsqrt(f) instead and been faster }
// 161
// 162 xInit = INT(CordicBase * f) { rounding type conversion to fixed point }
XINIT = _imp_INT(16384 * F);
// 163 %if debug # 0 %start
if (0 == 0) goto L_0018;
// 164 print string("xInit = "); print fl(CordicBase * f, 12)
_imp_PRINTSTRING(_imp_str_literal("xInit = "));
_imp_PRINTFL(16384 * F, 12);
// 165 print string(" = "); write(xInit, 0); newlines(2)
_imp_PRINTSTRING(_imp_str_literal(" = "));
_imp_WRITE(XINIT, 0);
_imp_NEWLINES(2);
// 166 %finish
L_0018:;
// 167
// 168 initialised = 1
INITIALISED = 1;
// 169 %finish { initialisation }
L_0011:;
// 170
// 171 ! Determine quadrant of incoming angle, translate to 1st quadrant.
// 172 ! Note use of previously calculated values CordicBase, etc. for speed.
// 173
// 174 theta = theta & (K64-1) { reduce to a single circle }
THETA = THETA & 65536 - 1;
// 175 %if theta < CordicBase %then quadrant = QUAD1 %and z = theta %c
if (THETA >= 16384) goto L_0019;
QUADRANT = 3;
Z = THETA;
goto L_001A;
L_0019:;
if (THETA >= 32768) goto L_001B;
QUADRANT = 2;
Z = 32768 - THETA;
goto L_001A;
L_001B:;
if (THETA >= 49152) goto L_001C;
QUADRANT = 0;
Z = THETA - 32768;
goto L_001A;
L_001C:;
QUADRANT = 1;
Z = -THETA;
L_001A:;
// 176+ %else %if theta < Quad2Boundary %then quadrant = QUAD2 %and z = Quad2Boundary - theta %c
// 177+ %else %if theta < Quad3Boundary %then quadrant = QUAD3 %and z = theta - Quad2Boundary %c
// 178+ %else quadrant = QUAD4 %and z = -theta
// 179
// 180 ! Initialize projections; Negate z, so same rotations taking angle z to 0 will take (x, y) = (xInit, 0) to (cos, sin).
// 181 z = z & (K32-1) { this is a bit dodgy on my part as there *are* cases where z is negative above }
Z = Z & 32768 - 1;
// 182 x = xInit; y = 0; z = -z
X = XINIT;
Y = 0;
Z = -Z;
// 183
// 184 %for i = 0, 1, NBITS-1 %cycle { Rotate NBITS times. }
// %for I = 0, 1, 14 - 1 %cycle
{
int _final = 14 - 1;
if (invalid_for_loop(0, 1, _final)) _imp_signal(5, 1, 0, "Illegal cycle I = 0, 1, 14 - 1");
I = -1;
L_001D:
if (I == _final) goto L_001E;
I += 1;
// 185 %if z < 0 %then z = z+ArcTan(i) %and yl = y + (x >> i) %and xl = x - (y >> i) { Counter-clockwise rotation. } %c
if (Z >= 0) goto L_001F;
Z = Z + ARCTAN[I];
YL = Y + ((unsigned int)X >> I);
XL = X - ((unsigned int)Y >> I);
goto L_0020;
L_001F:;
Z = Z - ARCTAN[I];
YL = Y - ((unsigned int)X >> I);
XL = X + ((unsigned int)Y >> I);
L_0020:;
// 186+ %else z = z-ArcTan(i) %and yl = y - (x >> i) %and xl = x + (y >> i) { Clockwise rotation. }
// 187 x = xl; y = yl { Put new projections into (x,y) for next go. }
X = XL;
Y = YL;
// 188 %repeat
goto L_001D;
}
L_001E:;
// 189
// 190 %if quadrant&1 # 0 %then cos = x %else cos = -x { Attach signs depending on quadrant. }
if (QUADRANT & 1 == 0) goto L_0021;
*COS = X;
goto L_0022;
L_0021:;
*COS = -X;
L_0022:;
// 191 %if quadrant&4 # 0 %then sin = y %else sin = -y
if (QUADRANT & 4 == 0) goto L_0023;
*SIN = Y;
goto L_0024;
L_0023:;
*SIN = -Y;
L_0024:;
// 192
// 193 xsin=sin {debug}
XSIN = *SIN;
// 194 sin = -sin %if Theta < K32
if (THETA >= 32768) goto L_0025;
*SIN = -*SIN;
L_0025:;
// 195 sin = sin & (K64-1); sin = sin!(\(K16-1)) %if sin > K16
*SIN = *SIN & 65536 - 1;
if (*SIN <= 16384) goto L_0026;
*SIN = *SIN | ~(16384 - 1);
L_0026:;
// 196
// 197 xcos=cos {debug}
XCOS = *COS;
// 198 cos = cos & (K64-1); cos = cos!(\(K16-1)) %if cos > K16
*COS = *COS & 65536 - 1;
if (*COS <= 16384) goto L_0027;
*COS = *COS | ~(16384 - 1);
L_0027:;
// 199
// 200 %if debug > 1 %start
if (0 <= 1) goto L_0028;
// 201 print string(" Theta ="); write(theta*1608//1024,8)
_imp_PRINTSTRING(_imp_str_literal(" Theta ="));
_imp_WRITE(((long)(THETA * 1608) / (long)1024), 8);
// 202 print string("; SIN(Theta) = "); phex(xsin);
_imp_PRINTSTRING(_imp_str_literal("; SIN(Theta) = "));
PHEX(XSIN);
// 203 print string("; COS(Theta) = "); phex(xcos);
_imp_PRINTSTRING(_imp_str_literal("; COS(Theta) = "));
PHEX(XCOS);
// 204 newline
_imp_NEWLINE();
// 205 %finish
L_0028:;
// 206
// 207 ! There is a bug in this implementation that fails near multiples of 90 degrees -
// 208 ! this test catches the broken cases and uses an approximation to repair the results :-(
// 209 ! I will remove these hacks later, if I ever find and fix the bug properly!
// 210 ! My suspicion is of overflow of a partial 16x16 multiplication somewhere, or maybe a '<<'.
// 211 %if xsin&16_FFFFC000 # 16_FFFFC000 %and xsin&16_FFFFC000 # 0 %then %start
if (XSIN & (-16384) == -16384) goto L_0029;
if (XSIN & (-16384) == 0) goto L_0029;
// 212 %if theta < 16_0400 %start
if (THETA >= 1024) goto L_002A;
// 213 sin = theta*1608//1024
*SIN = ((long)(THETA * 1608) / (long)1024);
// 214 %else %if 16_3C00 <= theta < 16_4000
goto L_002B;
L_002A:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (15360 > (tmp = THETA)) goto L_002C;
// Double-sided test on THETA (Part II)
if (tmp >= 16384) goto L_002C;
}
// 215 sin = K16 - (16_4000-theta)//25
*SIN = 16384 - ((long)(16384 - THETA) / (long)25);
// 216 %else %if 16_4000 <= theta < 16_4400
goto L_002B;
L_002C:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (16384 > (tmp = THETA)) goto L_002D;
// Double-sided test on THETA (Part II)
if (tmp >= 17408) goto L_002D;
}
// 217 sin = K16 - (theta-16_4000)//25
*SIN = 16384 - ((long)(THETA - 16384) / (long)25);
// 218 %else %if 16_7C00 <= theta < 16_8400
goto L_002B;
L_002D:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (31744 > (tmp = THETA)) goto L_002E;
// Double-sided test on THETA (Part II)
if (tmp >= 33792) goto L_002E;
}
// 219 sin = (16_8000-theta)*1608//1024
*SIN = ((long)((32768 - THETA) * 1608) / (long)1024);
// 220 %else %if 16_BC00 <= theta < 16_C000
goto L_002B;
L_002E:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (48128 > (tmp = THETA)) goto L_002F;
// Double-sided test on THETA (Part II)
if (tmp >= 49152) goto L_002F;
}
// 221 sin = -K16 + (16_C000-theta)//25
*SIN = (-16384) + ((long)(49152 - THETA) / (long)25);
// 222 %else %if 16_C000 <= theta < 16_C400
goto L_002B;
L_002F:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (49152 > (tmp = THETA)) goto L_0030;
// Double-sided test on THETA (Part II)
if (tmp >= 50176) goto L_0030;
}
// 223 sin = -K16 + (theta-16_C000)//25
*SIN = (-16384) + ((long)(THETA - 49152) / (long)25);
// 224 %else %if 16_FC00 <= theta
goto L_002B;
L_0030:;
if (64512 > THETA) goto L_0031;
// 225 sin = -(16_10000-theta)*1608//1024
*SIN = -(((long)((65536 - THETA) * 1608) / (long)1024));
// 226 %else
goto L_002B;
L_0031:;
// 227 print string("Oops, missed a case: sin "); phex(theta); newline; %stop
_imp_PRINTSTRING(_imp_str_literal("Oops, missed a case: sin "));
PHEX(THETA);
_imp_NEWLINE();
_imp_stop(0);
// 228 %finish
L_002B:;
// 229 %finish
L_0029:;
// 230
// 231 %if xcos&16_FFFFC000 # 16_FFFFC000 %and xcos&16_FFFFC000 # 0 %then %start
if (XCOS & (-16384) == -16384) goto L_0032;
if (XCOS & (-16384) == 0) goto L_0032;
// 232 %if theta < 16_0400 %start
if (THETA >= 1024) goto L_0033;
// 233 cos = K16 - (theta)//25
*COS = 16384 - ((long)THETA / (long)25);
// 234 %else %if 16_3C00 <= theta < 16_4400
goto L_0034;
L_0033:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (15360 > (tmp = THETA)) goto L_0035;
// Double-sided test on THETA (Part II)
if (tmp >= 17408) goto L_0035;
}
// 235 cos = (16_4000-theta)*1608//1024
*COS = ((long)((16384 - THETA) * 1608) / (long)1024);
// 236 %else %if 16_7C00 <= theta < 16_8000
goto L_0034;
L_0035:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (31744 > (tmp = THETA)) goto L_0036;
// Double-sided test on THETA (Part II)
if (tmp >= 32768) goto L_0036;
}
// 237 cos = -K16 + (16_8000-theta)//25
*COS = (-16384) + ((long)(32768 - THETA) / (long)25);
// 238 %else %if 16_8000 <= theta < 16_8400
goto L_0034;
L_0036:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (32768 > (tmp = THETA)) goto L_0037;
// Double-sided test on THETA (Part II)
if (tmp >= 33792) goto L_0037;
}
// 239 cos = -K16 + (theta-16_8000)//25
*COS = (-16384) + ((long)(THETA - 32768) / (long)25);
// 240 %else %if 16_BC00 <= theta < 16_C400
goto L_0034;
L_0037:;
{
// Double-sided test on THETA (Part I)
int tmp;
if (48128 > (tmp = THETA)) goto L_0038;
// Double-sided test on THETA (Part II)
if (tmp >= 50176) goto L_0038;
}
// 241 cos = (theta-16_C000)*1608//1024
*COS = ((long)((THETA - 49152) * 1608) / (long)1024);
// 242 %else %if 16_FC00 <= theta
goto L_0034;
L_0038:;
if (64512 > THETA) goto L_0039;
// 243 cos = K16 - (16_10000-theta)//25
*COS = 16384 - ((long)(65536 - THETA) / (long)25);
// 244 %else
goto L_0034;
L_0039:;
// 245 print string("Oops, missed a case: cos "); phex(theta); newline; %stop
_imp_PRINTSTRING(_imp_str_literal("Oops, missed a case: cos "));
PHEX(THETA);
_imp_NEWLINE();
_imp_stop(0);
// 246 %finish
L_0034:;
// 247 %finish
L_0032:;
// 248 %end
return;
} /* end of SINCOS */
// ?HALFBASE unused
// 249
// 250 %external %real %fn %spec sin %alias "sinf" (%real angle) ;! use C's math lib to check results.
extern float SIN(float ANGLE) __asm__("sinf");
// 251 %external %real %fn %spec cos %alias "cosf" (%real angle)
extern float COS(float ANGLE) __asm__("cosf");
// 252
// 253 %real Theta
float THETA;
// 254 %integer i, sinTheta, cosTheta, CheckSinTheta, CheckCosTheta, sinErr = 0, cosErr = 0, err
int I;
int SINTHETA;
int COSTHETA;
int CHECKSINTHETA;
int CHECKCOSTHETA;
int SINERR;
SINERR = 0;
int COSERR;
COSERR = 0;
int ERR;
// 255
// 256 %routine pround(%integer f, n)
void PROUND(int F, int N) {
// 257 ! This is a quick (and rather ugly) hack due to the current Imp compiler
// 258 ! not implementing all of the usual floating point I/O library calls yet
// 259
// 260 %real r, sign = 1.0
float R;
float SIGN;
SIGN = 1.0;
// 261 %integer tens = 0, p
int TENS;
TENS = 0;
int P;
// 262
// 263 %if f=0 %start
if (F != 0) goto L_003A;
// 264 r = 0
R = 0;
// 265 %else
goto L_003B;
L_003A:;
// 266 f = -f %and sign = -1.0 %if f < 0
if (F >= 0) goto L_003C;
F = -F;
SIGN = -1.0;
L_003C:;
// 267 r = f / K16
R = (double)(F / 16384);
// 268 r = r * 10.0 %and tens = tens+1 %until 100 <= INT PT(r) < 1000
L_003D:;
R = R * 10.0;
TENS = TENS + 1;
{
// Double-sided test on _imp_INTPT(R) (Part I)
int tmp;
if (100 > (tmp = _imp_INTPT(R))) goto L_0014;
// Double-sided test on _imp_INTPT(R) (Part II)
if (tmp < 1000) goto L_003E;
}
L_0014:;
goto L_003D;
L_003E:;
// 269 p = tens
P = TENS;
// 270 r = INT(r) { round to 3 significant digits }
R = _imp_INT(R);
// 271 r = r / 10.0 %and tens = tens-1 %while tens > 0
L_003F:;
if (TENS <= 0) goto L_0040;
R = (double)(R / 10.0);
TENS = TENS - 1;
goto L_003F;
L_0040:;
// 272 r = r + 0.000001 { handle real values not exactly representable in decimal }
R = R + 0.000001;
// 273 %finish
L_003B:;
// 274 spaces(n - (p+3))
_imp_SPACES(N - (P + 3));
// 275 %if sign < 0 %then print symbol('-') %else print symbol(' ')
if (SIGN >= 0) goto L_0041;
_imp_PRINTSYMBOL(45);
goto L_0042;
L_0041:;
_imp_PRINTSYMBOL(32);
L_0042:;
// 276 print fl(r, p+2) { library call truncates, doesn't round :-( }
_imp_PRINTFL(R, P + 2);
// 277 %end
return;
} /* end of PROUND */
// 278
// 279 ! We get about 3 significant digits of accuracy
// 280 print string("Note: this CORDIC was implemented to only 14 bits of accuracy for 16 bit machines,"); newline
_imp_PRINTSTRING(
_imp_str_literal("Note: this CORDIC was implemented to only 14 bits of accuracy for 16 bit machines,"));
_imp_NEWLINE();
// 281 print string(" so we will report results to 3 significant digits."); newlines(2)
_imp_PRINTSTRING(_imp_str_literal(" so we will report results to 3 significant digits."));
_imp_NEWLINES(2);
// 282
// 283 SinCos(INT(-9*K64/(2*PI)), sinTheta, cosTheta)
SINCOS(_imp_INT(-((double)((9 * 65536) / (2 * PI)))), /*?*/ &SINTHETA, /*?*/ &COSTHETA);
// 284 printstring("sin and cos of -9 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
_imp_PRINTSTRING(_imp_str_literal("sin and cos of -9 is "));
PROUND(SINTHETA, 6);
_imp_SPACE();
PROUND(COSTHETA, 6);
_imp_NEWLINE();
// 285
// 286 SinCos(INT(0*K64/(2*PI)), sinTheta, cosTheta)
SINCOS(_imp_INT((double)((0 * 65536) / (2 * PI))), /*?*/ &SINTHETA, /*?*/ &COSTHETA);
// 287 printstring("sin and cos of 0 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
_imp_PRINTSTRING(_imp_str_literal("sin and cos of 0 is "));
PROUND(SINTHETA, 6);
_imp_SPACE();
PROUND(COSTHETA, 6);
_imp_NEWLINE();
// 288
// 289 SinCos(INT(1.5*K64/(2*PI)), sinTheta, cosTheta)
SINCOS(_imp_INT((double)((1.5 * 65536) / (2 * PI))), /*?*/ &SINTHETA, /*?*/ &COSTHETA);
// 290 printstring("sin and cos of 1.5 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
_imp_PRINTSTRING(_imp_str_literal("sin and cos of 1.5 is "));
PROUND(SINTHETA, 6);
_imp_SPACE();
PROUND(COSTHETA, 6);
_imp_NEWLINE();
// 291
// 292 SinCos(INT(6*K64/(2*PI)), sinTheta, cosTheta)
SINCOS(_imp_INT((double)((6 * 65536) / (2 * PI))), /*?*/ &SINTHETA, /*?*/ &COSTHETA);
// 293 printstring("sin and cos of 6 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline
_imp_PRINTSTRING(_imp_str_literal("sin and cos of 6 is "));
PROUND(SINTHETA, 6);
_imp_SPACE();
PROUND(COSTHETA, 6);
_imp_NEWLINE();
// 294
// 295
// 296 %if debug # 0 %start
if (0 == 0) goto L_0043;
// 297 %for i = 0, 4096, K64 %cycle { A circle is 64K units }
// %for I = 0, 4096, 65536 %cycle
{
I = -4096;
L_0044:
if (I == 65536) goto L_0045;
I += 4096;
// 298 Theta = i*2*PI/K64 { A circle is 2Pi radians }
THETA = (double)((I * 2 * PI) / 65536);
// 299
// 300 SinCos(i, sinTheta, cosTheta)
SINCOS(I, /*?*/ &SINTHETA, /*?*/ &COSTHETA);
// 301
// 302 print string("Cordic: Theta = "); phex(i)
_imp_PRINTSTRING(_imp_str_literal("Cordic: Theta = "));
PHEX(I);
// 303 print string("; Sin(Theta) = "); write(sinTheta, 0)
_imp_PRINTSTRING(_imp_str_literal("; Sin(Theta) = "));
_imp_WRITE(SINTHETA, 0);
// 304 print string("; Cos(Theta) = "); write(cosTheta, 0)
_imp_PRINTSTRING(_imp_str_literal("; Cos(Theta) = "));
_imp_WRITE(COSTHETA, 0);
// 305 newline;
_imp_NEWLINE();
// 306
// 307 CheckSinTheta = INT(sin(Theta)*K16); CheckCosTheta = INT(cos(Theta)*K16);
CHECKSINTHETA = _imp_INT(SIN(THETA) * 16384);
CHECKCOSTHETA = _imp_INT(COS(THETA) * 16384);
// 308
// 309 print string("Floating point: Theta = "); phex(INT(Theta/(2*PI)*K64))
_imp_PRINTSTRING(_imp_str_literal("Floating point: Theta = "));
PHEX(_imp_INT((double)(THETA / (2 * PI)) * 65536));
// 310 print string("; Sin(Theta) = "); write(CheckSinTheta, 0)
_imp_PRINTSTRING(_imp_str_literal("; Sin(Theta) = "));
_imp_WRITE(CHECKSINTHETA, 0);
// 311 print string("; Cos(Theta) = "); write(CheckCosTheta, 0)
_imp_PRINTSTRING(_imp_str_literal("; Cos(Theta) = "));
_imp_WRITE(CHECKCOSTHETA, 0);
// 312 newlines(2)
_imp_NEWLINES(2);
// 313
// 314 err = CheckSinTheta - sinTheta; err = -err %if err < 0; sinErr = sinErr + err
ERR = CHECKSINTHETA - SINTHETA;
if (ERR >= 0) goto L_0047;
ERR = -ERR;
L_0047:;
SINERR = SINERR + ERR;
// 315 err = CheckCosTheta - cosTheta; err = -err %if err < 0; cosErr = cosErr + err
ERR = CHECKCOSTHETA - COSTHETA;
if (ERR >= 0) goto L_0048;
ERR = -ERR;
L_0048:;
COSERR = COSERR + ERR;
// 316
// 317 %repeat
goto L_0044;
}
L_0045:;
// 318
// 319 { Use this to test if algorithmic tweaks improve the accuracy }
// 320 print string("Cumulative error: sin = "); write(sinErr, 0)
_imp_PRINTSTRING(_imp_str_literal("Cumulative error: sin = "));
_imp_WRITE(SINERR, 0);
// 321 print string(", cos = "); write(cosErr, 0)
_imp_PRINTSTRING(_imp_str_literal(", cos = "));
_imp_WRITE(COSERR, 0);
// 322 newline
_imp_NEWLINE();
// 323
// 324 %finish
L_0043:;
// 325 %end %of %program
_imp_stop(0);
return 0;
end_0:;
}