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 27 28 %const %integer debug = 1 { 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) 32 %const %long %real r1= 1.2595802263029547 @ 1 33 %const %long %real r2=-8.6186317517509520 @ 1 34 %const %long %real r3=-1.2766919133361079 @ 0 35 %const %long %real r4=-8.3921038065840512 @ -2 36 %const %long %real s1= 2.7096164294378656 @ 1 37 %const %long %real s2= 6.5581320451487386 @ 0 38 %const %long %real s3= 2.1441643116703661 @ 0 39 %const %long %real s4= 1.2676256708212610 @ 0 40 %const %long %real rt3= 1.7320508075688772 @ 0 41 %const %long %real piby6= 5.2359877559829887 @ -1 42 %const %long %real piby2m1= 5.7079632679489661 @ -1 43 %const %long %real rt3m1=7.3205080756887728 @ -1 44 %const %long %real tanpiby12= 2.6794919243112271 @ -1 45 %long %real xx1,xsq,constant=0 46 %integer sign=0,inv=0 47 48 %if x1<0 %then sign=1 %and xx1=-x1 %else xx1=x1 49 %if xx1>1.0 %then xx1=1.0/xx1 %and inv=1 50 %if xx1>tanpiby12 %then xx1=(rt3m1*xx1-1.0+xx1)/(xx1+rt3) %and constant=piby6 51 xsq=xx1*xx1 52 xx1 = xx1*(r1/(xsq+s1+r2/(xsq+s2+r3/(xsq+s3+r4/(xsq+s4))))) 53 xx1 = xx1 + constant 54 %if inv=1 %then xx1=1.0-xx1+piby2m1 55 %if sign=1 %then xx1=-xx1 56 %result=xx1 { shorten from %long %real down to %real when returning result } 57 %end 58 59 !%external %real %fn %spec sqrt %alias "sqrtf" (%real angle) 60 %real %fn sqrt(%long %real X) 61 %integer %fn approx integer sqrt(%integer X) 62 %integer k = 1, result = 1 63 k = k+1 %and result = k*k %while result <= X 64 %result = k-1 {note 'off by 1' bug in original C version due to returning k--} 65 %end 66 %integer I 67 %long %real prev A, A, epsilon = X / 100 {accurate enough due to .14 fixed point} 68 %if X = 2.0 %then %result = 1.41421356237 %else I = approx integer sqrt(INT PT(X)) 69 %result = I %if X = I*I { exact root } 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 72 %result = A 73 %end 74 75 %real 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) 80 %own %string (16) hex = "0123456789ABCDEF" 81 %integer i 82 print symbol(charno(hex, (x >> (i<<2))&15+1)) %for i = 7, -1, 0 83 %end 84 85 %routine SinCos({unsigned} %integer theta, %integer %name sin, %integer %name 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 } 96 %own %integer xInit { initial x projection } 97 98 %integer xcos,xsin { used to patch a problem that happens near the zeroes and extremes of the results } 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 } 112 %integer z { incoming angle moved to 1st quad } 113 %integer i { to index rotations - one per bit } 114 %integer x, y { projections onto axes } 115 %integer xl, yl { projections of rotated vector } 116 117 %own %integer initialised = 0 118 %if initialised = 0 %start 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 } 135 %integer powr { powers of 2 up to 2^(2*(NBITS-1)) } 136 137 { ArcTans are diminishingly small angles. } 138 powr = 1 139 %for i = 0, 1, NBITS-1 %cycle 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) 143 %if debug > 1 %start 144 print string("Setting ArcTan("); write(i,0); print string(") to "); phex(ArcTan(i)); newline 145 %finish 146 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 151 f = 1.0 152 153 powr = 1 154 %for i = 0, 1, NBITS-1 %cycle 155 f = (f * (powr + 1)) / powr 156 powr = powr << 2 157 %repeat 158 159 f = 1.0/sqrt(f); { sqrt is expensive but only done once during initialisation } 160 { Note could have implemented f = rsqrt(f) instead and been faster } 161 162 xInit = INT(CordicBase * f) { rounding type conversion to fixed point } 163 %if debug # 0 %start 164 print string("xInit = "); print fl(CordicBase * f, 12) 165 print string(" = "); write(xInit, 0); newlines(2) 166 %finish 167 168 initialised = 1 169 %finish { initialisation } 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 } 175 %if theta < CordicBase %then quadrant = QUAD1 %and z = theta %c 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 } 182 x = xInit; y = 0; z = -z 183 184 %for i = 0, 1, NBITS-1 %cycle { Rotate NBITS times. } 185 %if z < 0 %then z = z+ArcTan(i) %and yl = y + (x >> i) %and xl = x - (y >> i) { Counter-clockwise rotation. } %c 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. } 188 %repeat 189 190 %if quadrant&1 # 0 %then cos = x %else cos = -x { Attach signs depending on quadrant. } 191 %if quadrant&4 # 0 %then sin = y %else sin = -y 192 193 xsin=sin {debug} 194 sin = -sin %if Theta < K32 195 sin = sin & (K64-1); sin = sin!(\(K16-1)) %if sin > K16 196 197 xcos=cos {debug} 198 cos = cos & (K64-1); cos = cos!(\(K16-1)) %if cos > K16 199 200 %if debug > 1 %start 201 print string(" Theta ="); write(theta*1608//1024,8) 202 print string("; SIN(Theta) = "); phex(xsin); 203 print string("; COS(Theta) = "); phex(xcos); 204 newline 205 %finish 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 212 %if theta < 16_0400 %start 213 sin = theta*1608//1024 214 %else %if 16_3C00 <= theta < 16_4000 215 sin = K16 - (16_4000-theta)//25 216 %else %if 16_4000 <= theta < 16_4400 217 sin = K16 - (theta-16_4000)//25 218 %else %if 16_7C00 <= theta < 16_8400 219 sin = (16_8000-theta)*1608//1024 220 %else %if 16_BC00 <= theta < 16_C000 221 sin = -K16 + (16_C000-theta)//25 222 %else %if 16_C000 <= theta < 16_C400 223 sin = -K16 + (theta-16_C000)//25 224 %else %if 16_FC00 <= theta 225 sin = -(16_10000-theta)*1608//1024 226 %else 227 print string("Oops, missed a case: sin "); phex(theta); newline; %stop 228 %finish 229 %finish 230 231 %if xcos&16_FFFFC000 # 16_FFFFC000 %and xcos&16_FFFFC000 # 0 %then %start 232 %if theta < 16_0400 %start 233 cos = K16 - (theta)//25 234 %else %if 16_3C00 <= theta < 16_4400 235 cos = (16_4000-theta)*1608//1024 236 %else %if 16_7C00 <= theta < 16_8000 237 cos = -K16 + (16_8000-theta)//25 238 %else %if 16_8000 <= theta < 16_8400 239 cos = -K16 + (theta-16_8000)//25 240 %else %if 16_BC00 <= theta < 16_C400 241 cos = (theta-16_C000)*1608//1024 242 %else %if 16_FC00 <= theta 243 cos = K16 - (16_10000-theta)//25 244 %else 245 print string("Oops, missed a case: cos "); phex(theta); newline; %stop 246 %finish 247 %finish 248 %end ?HALFBASE unused 249 250 !%external %real %fn %spec sin %alias "sinf" (%real angle) ;! use C's math lib to check results. 251 !%external %real %fn %spec cos %alias "cosf" (%real angle) 252 253 %real Theta 254 %integer i, sinTheta, cosTheta, CheckSinTheta, CheckCosTheta, sinErr = 0, cosErr = 0, err 255 256 %routine pround(%integer f, 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 261 %integer tens = 0, p 262 263 %if f=0 %start 264 r = 0 265 %else 266 f = -f %and sign = -1.0 %if f < 0 267 r = f / K16 268 r = r * 10.0 %and tens = tens+1 %until 100 <= INT PT(r) < 1000 269 p = tens 270 r = INT(r) { round to 3 significant digits } 271 r = r / 10.0 %and tens = tens-1 %while tens > 0 272 r = r + 0.000001 { handle real values not exactly representable in decimal } 273 %finish 274 spaces(n - (p+3)) 275 %if sign < 0 %then print symbol('-') %else print symbol(' ') 276 print fl(r, p+2) { library call truncates, doesn't round :-( } 277 %end 278 279 %on %event * %start 280 newline 281 %if event_event = 14 %and event_sub{event} = 1 %start 282 ! These are generated outside of the Imp77 environment, such as by Valgrind or GCC's Undefined Behaviour detection (ubsan). 283 %if event_info{extra} = 4 %start 284 ! SIGILL 285 print string("Linux Signal SIGILL trapped at ") 286 print string(event file) 287 print string(" line") 288 write(event line, 0) 289 print string(" - this was probably raised by ubsan (gcc -fsanitize-trap=undefined)") 290 newline 291 %else %if event_info{extra} = 8 292 ! SIGFPE 293 print string("Linux Signal SIGFPE trapped at ") 294 print string(event file) 295 print string(" line") 296 write(event line, 0) 297 print string(" - this is a floating point error (such as divide by zero)") 298 newline 299 %else %if event_info{extra} = 8 300 ! SIGSEGV 301 print string("Linux Signal SIGSEGV trapped at ") 302 print string(event file) 303 print string(" line") 304 write(event line, 0) 305 print string(" - this may be from a NULL pointer or accessing past the end of an array") 306 newline 307 %else 308 ! unknown 309 print string("Linux Signal "); write(event_info{extra}, 0); print string(" trapped."); newlines(2) 310 %finish 311 %else 312 print string("Imp77 %signal ") 313 write(event_event,0); print string(", ") 314 write(event_sub{event},0); print string(", ") 315 write(event_info{extra},0) 316 print string(" raised in ") 317 print string(event file) 318 print string(" line") 319 write(event line, 0) 320 newline 321 %finish 322 print string(event message) 323 %stop 324 %finish 325 326 ! We get about 3 significant digits of accuracy 327 print string("Note: this CORDIC was implemented to only 14 bits of accuracy for 16 bit machines,"); newline 328 print string(" so we will report results to 3 significant digits."); newlines(2) 329 330 SinCos(INT(-9*K64/(2*PI)), sinTheta, cosTheta) 331 printstring("sin and cos of -9 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline 332 333 SinCos(INT(0*K64/(2*PI)), sinTheta, cosTheta) 334 printstring("sin and cos of 0 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline 335 336 SinCos(INT(1.5*K64/(2*PI)), sinTheta, cosTheta) 337 printstring("sin and cos of 1.5 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline 338 339 SinCos(INT(6*K64/(2*PI)), sinTheta, cosTheta) 340 printstring("sin and cos of 6 is "); pround(sinTheta,6); space; pround(cosTheta,6); newline 341 342 343 %if debug # 0 %start 344 %for i = 0, 4096, K64 %cycle { A circle is 64K units } 345 Theta = i*2*PI/K64 { A circle is 2Pi radians } 346 347 SinCos(i, sinTheta, cosTheta) 348 349 print string("Cordic: Theta = "); phex(i) 350 print string("; Sin(Theta) = "); write(sinTheta, 0) 351 print string("; Cos(Theta) = "); write(cosTheta, 0) 352 newline; 353 354 CheckSinTheta = INT(sin(Theta)*K16); CheckCosTheta = INT(cos(Theta)*K16); 355 356 print string("Floating point: Theta = "); phex(INT(Theta/(2*PI)*K64)) 357 print string("; Sin(Theta) = "); write(CheckSinTheta, 0) 358 print string("; Cos(Theta) = "); write(CheckCosTheta, 0) 359 newlines(2) 360 361 err = CheckSinTheta - sinTheta; err = -err %if err < 0; sinErr = sinErr + err 362 err = CheckCosTheta - cosTheta; err = -err %if err < 0; cosErr = cosErr + err 363 364 %repeat 365 366 { Use this to test if algorithmic tweaks improve the accuracy } 367 print string("Cumulative error: sin = "); write(sinErr, 0) 368 print string(", cos = "); write(cosErr, 0) 369 newline 370 371 %finish 372 %end %of %program ?K8 unused 297 Statements compiled