| 1 | REM |
| 2 | REM fixedPt.bs |
| 3 | REM |
| 4 | REM Table-based trig functions |
| 5 | REM |
| 6 | REM © 1995-1998 Straylight |
| 7 | REM |
| 8 | |
| 9 | REM ----- Licensing note ---------------------------------------------------- |
| 10 | REM |
| 11 | REM This file is part of Straylight's Sapphire library. |
| 12 | REM |
| 13 | REM Sapphire is free software; you can redistribute it and/or modify |
| 14 | REM it under the terms of the GNU General Public License as published by |
| 15 | REM the Free Software Foundation; either version 2, or (at your option) |
| 16 | REM any later version |
| 17 | REM |
| 18 | REM Sapphire is distributed in the hope that it will be useful, |
| 19 | REM but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 20 | REM MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 21 | REM GNU General Public License for more details. |
| 22 | REM |
| 23 | REM You should have received a copy of the GNU General Public License |
| 24 | REM along with Sapphire. If not, write to the Free Software Foundation, |
| 25 | REM 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
| 26 | |
| 27 | REM --- How the table works --- |
| 28 | REM |
| 29 | REM We represent a number in 16.16 fixed point. We set up a table of |
| 30 | REM arctan values for values of x between 0 and 1. All the others may be |
| 31 | REM calculated from this, hopefully. Certainly, this is enough for |
| 32 | REM calculating polar angles, which is the really interesting bit. |
| 33 | REM |
| 34 | REM For niceness purposes, the values are actually stored in degrees, rather |
| 35 | REM than radians. Division by 360 isn't a major difficulty, so it's not |
| 36 | REM hard to convert back again. |
| 37 | |
| 38 | REM --- Set up some variables --- |
| 39 | |
| 40 | ON ERROR ERROR 0,REPORT$+" ["+STR$(ERL)+"]" |
| 41 | bas%=TRUE :REM Generate BAS code |
| 42 | bas%=bas% |
| 43 | |
| 44 | REM --- BAS library --- |
| 45 | |
| 46 | IF bas% THEN |
| 47 | LIBRARY "libs:bas" |
| 48 | PROCbas_init |
| 49 | ENDIF |
| 50 | |
| 51 | REM --- A little bit of test code --- |
| 52 | |
| 53 | IF bas% THEN |
| 54 | PROCbas_aofInit(&10000) |
| 55 | o_base%=4 |
| 56 | o_lim%=6 |
| 57 | ELSE |
| 58 | DIM code% 10240 |
| 59 | o_base%=0 |
| 60 | o_lim%=2 |
| 61 | ENDIF |
| 62 | |
| 63 | REM --- General parameters --- |
| 64 | |
| 65 | scalebits% = 16 :REM Avoiding tweaking this: it's part of the interface |
| 66 | scale% = 1 << scalebits% |
| 67 | |
| 68 | REM --- Parameters for the tan table -- |
| 69 | |
| 70 | atn_tblbits% = 7 :REM TWEAK ME! |
| 71 | atn_entries% = 1 << atn_tblbits% |
| 72 | atn_shift% = scalebits% - atn_tblbits% |
| 73 | atn_revshift% = 32 - atn_shift% |
| 74 | atn_interpoff% = 1 << (atn_shift% - 1) |
| 75 | |
| 76 | REM --- Parameters for the sin table --- |
| 77 | |
| 78 | sin_tblbits% = 6 :REM TWEAK ME! sin_tblbits may be at most 8 |
| 79 | sin_entries% = 1 << sin_tblbits% |
| 80 | sin_shift% = 30 - sin_tblbits% |
| 81 | sin_modmask% = (sin_entries% - 1) << sin_shift% |
| 82 | sin_interpoff% = 1 << (sin_shift% - 1) |
| 83 | |
| 84 | REM --- Start the assembly --- |
| 85 | |
| 86 | FOR o%=o_base% TO o_lim% STEP 2 :REM Standard two-pass assembly loop |
| 87 | IF bas%=0 THEN P%=code% :REM Start generating code in buffer |
| 88 | |
| 89 | IF bas% THEN |
| 90 | [ OPT o% |
| 91 | FNpass |
| 92 | |
| 93 | FNarea("Sapphire$$Code","CODE,READONLY") |
| 94 | |
| 95 | FNimport("div_unsigned") |
| 96 | FNimport("div_u64x32") |
| 97 | ] |
| 98 | ENDIF |
| 99 | |
| 100 | [ OPT o% :REM Start assembly |
| 101 | |
| 102 | ; --- arctan --- |
| 103 | ; |
| 104 | ; On entry R0 == x, in 16.16 fixed point form |
| 105 | ; |
| 106 | ; On exit R0 == arctan x, in degrees, in 16.16 fixed point |
| 107 | ; |
| 108 | ; Use Calculates arctan x, hopefully fairly swiftly. The |
| 109 | ; accuracy of the result is open to doubt, although |
| 110 | ; it's usually good to about 3 significant figures. |
| 111 | ; It uses a small lookup table and linear interpolation |
| 112 | ; to calculate the result. |
| 113 | |
| 114 | .arctan STMFD R13!,{R1-R3,R14} ;Save some work registers |
| 115 | |
| 116 | ; --- Set some initial things up --- |
| 117 | |
| 118 | MOV R3,#0 ;Clear out my flags register |
| 119 | |
| 120 | ; --- Now sort out the argument --- |
| 121 | |
| 122 | CMP R0,#0 ;Is the argument negative? |
| 123 | ORRLT R3,R3,#1 ;Yes -- set the `negative' bit |
| 124 | RSBLT R0,R0,#0 ;And make the argument positive |
| 125 | |
| 126 | CMP R0,#scale% ;Is the argument bigger than 1? |
| 127 | BLE atn_skip ;No; skip ahead a bit |
| 128 | |
| 129 | ORR R3,R3,#2 ;Set the `reciprocal' bit |
| 130 | ] |
| 131 | IF scalebits% < 16 THEN |
| 132 | [ OPT o% |
| 133 | MOV R1,R0 ;Stand by to do some division |
| 134 | MOV R0,#1 << (scalebits% * 2) |
| 135 | BL div_unsigned |
| 136 | ] |
| 137 | ELSE |
| 138 | [ OPT o% |
| 139 | MOV R2,R0 ;Stand by to do some division |
| 140 | MOV R0,#0 |
| 141 | MOV R1,#1 << (scalebits% * 2 - 32) |
| 142 | BL div_u64x32 |
| 143 | ] |
| 144 | ENDIF |
| 145 | [ OPT o% |
| 146 | |
| 147 | ; --- Look up the arctan value --- |
| 148 | |
| 149 | .atn_skip ADR R14,atn_table ;Load the address of the table |
| 150 | MOV R1,R0,LSL #atn_revshift% ;Get bottom bits in R1 |
| 151 | MOV R1,R1,LSR #atn_revshift% ;Shift back to bit 0 |
| 152 | MOV R0,R0,LSR #atn_shift% ;Get the table index |
| 153 | ADD R14,R14,R0,LSL #2 ;Get the address of the item |
| 154 | LDR R2,[R14,#0] ;Locate the lower entry and load |
| 155 | LDR R0,[R14,#4] ;Locate the upper entry and load |
| 156 | SUB R0,R0,R2 ;Get the difference between them |
| 157 | MUL R0,R1,R0 ;Multiply this by the x offset |
| 158 | ADD R0,R0,#atn_interpoff% ;Round to nearest |
| 159 | ADD R0,R2,R0,LSR #atn_shift% ;Do interpolation |
| 160 | |
| 161 | ; --- Now postprocess this result nicely --- |
| 162 | |
| 163 | TST R3,#2 ;Did I have to reciprocal it? |
| 164 | RSBNE R0,R0,#90*scale% ;Yes -- work out the correct one |
| 165 | TST R3,#1 ;Did I have to negate the value? |
| 166 | RSBNE R0,R0,#0 ;Yes -- negate the result |
| 167 | |
| 168 | LDMFD R13!,{R1-R3,PC}^ ;Return to caller |
| 169 | |
| 170 | ; --- pol --- |
| 171 | ; |
| 172 | ; On entry R0 == x coordinate |
| 173 | ; R1 == y coordinate |
| 174 | ; |
| 175 | ; On exit R0 == angle in degrees, in 16.16 form |
| 176 | ; |
| 177 | ; Use Calculates the angle a vector makes with the +ve x axis. |
| 178 | ; The angle is given in degrees, rather than radians, |
| 179 | ; although this isn't really overly significant, it just |
| 180 | ; makes it slightly easier to work with, because it's |
| 181 | ; bigger. |
| 182 | ; |
| 183 | ; This routine uses the arctan table and linear |
| 184 | ; interpolation, so it's fairly quick, but the accuracy |
| 185 | ; of its results is questionable. |
| 186 | |
| 187 | .pol STMFD R13!,{R1-R3,R14} ;Save some registers away |
| 188 | MOV R3,#0 ;Clear out my status flags |
| 189 | |
| 190 | ; --- Preprocess the arguments --- |
| 191 | |
| 192 | CMP R0,#0 ;Is x less than zero? |
| 193 | RSBLT R0,R0,#0 ;Yes -- make it positive |
| 194 | ORRLT R3,R3,#1 ;And remember that we did this |
| 195 | |
| 196 | CMP R1,#0 ;Is y less than zero? |
| 197 | RSBLT R1,R1,#0 ;Yes -- make it positive |
| 198 | ORRLT R3,R3,#2 ;And remember we did this |
| 199 | |
| 200 | CMP R0,R1 ;Is x bigger than y? |
| 201 | EORGT R0,R0,R1 ;Yes -- then swap them round |
| 202 | EORGT R1,R0,R1 |
| 203 | EORGT R0,R0,R1 |
| 204 | ORRGT R3,R3,#4 ;And remember we did this |
| 205 | |
| 206 | ; --- Handle silly case where point is at (0,0) --- |
| 207 | |
| 208 | CMP R1,#0 ;Are we about to divide by 0? |
| 209 | LDMEQFD R13!,{R1-R3,PC}^ ;Yes -- return now -- R0 is 0 |
| 210 | |
| 211 | ; --- Now work out arctan(R0/R1) --- |
| 212 | |
| 213 | MOV R2,R1 ;Get the divisor |
| 214 | MOV R1,R0,LSR #32 - scalebits% ;Sort out dividend |
| 215 | MOV R0,R0,LSL #scalebits% |
| 216 | BL div_u64x32 ;Work out the ratio nicely |
| 217 | |
| 218 | ADR R14,atn_table ;Load the address of the table |
| 219 | MOV R1,R0,LSL #atn_revshift% ;Get bottom bits in R1 |
| 220 | MOV R1,R1,LSR #atn_revshift% ;Shift back to bit 0 |
| 221 | MOV R0,R0,LSR #atn_shift% ;Get correct table index |
| 222 | ADD R14,R14,R0,LSL #2 ;Get the address of the item |
| 223 | LDR R2,[R14,#0] ;Locate the lower entry and load |
| 224 | LDR R0,[R14,#4] ;Locate the upper entry and load |
| 225 | SUB R0,R0,R2 ;Get the difference between them |
| 226 | MUL R0,R1,R0 ;Multiply this by the x offset |
| 227 | ADD R0,R0,#atn_interpoff% ;Just round to nearest |
| 228 | ADD R0,R2,R0,LSR #atn_shift% ;And add to the base |
| 229 | |
| 230 | ; --- arctan is now in R0 -- process it for angle --- |
| 231 | |
| 232 | TST R3,#4 ;Did we swap x and y round? |
| 233 | RSBEQ R0,R0,#90*scale% ;No -- compensate for this |
| 234 | TST R3,#1 ;Was x negative? |
| 235 | RSBNE R0,R0,#180*scale% ;Yes -- push it to left side |
| 236 | TST R3,#2 ;Was y negative? |
| 237 | RSBNE R0,R0,#360*scale% ;Yes -- push it to bottom |
| 238 | |
| 239 | LDMFD R13!,{R1-R3,PC}^ ;Return to caller |
| 240 | |
| 241 | ; --- arctan table --- |
| 242 | |
| 243 | .atn_table |
| 244 | ] |
| 245 | |
| 246 | FOR i%=0 TO atn_entries% :REM Go through each index value |
| 247 | a=DEG ATN(i%/atn_entries%) :REM Calculate the arctan value |
| 248 | [opt o%:dcd a*scale%:] :REM Store it in the table nicely |
| 249 | NEXT |
| 250 | |
| 251 | [ OPT o% |
| 252 | |
| 253 | ; --- sin --- |
| 254 | ; |
| 255 | ; On entry R0 == angle in degrees, in 16.16 form |
| 256 | ; |
| 257 | ; On exit R0 == sin of angle, in 16.16 form |
| 258 | ; |
| 259 | ; Use Calculates a sin of an angle with a degree of swiftness |
| 260 | ; and a lot less accuracy. |
| 261 | |
| 262 | .cos RSB R0,R0,#90*scale% |
| 263 | |
| 264 | .sin STMFD R13!,{R1-R3,R14} ;Save a job load of registers |
| 265 | |
| 266 | ; --- How it all works --- |
| 267 | ; |
| 268 | ; Having angles in degrees imposes inconvenient units |
| 269 | ; on use, forcing us to do all sorts of horrible things |
| 270 | ; with division by 360. |
| 271 | ; |
| 272 | ; Hence, we translate the degree units into nice things |
| 273 | ; which represent a right angle as a power of two, |
| 274 | ; say 2^16. |
| 275 | |
| 276 | MOV R1,R0,LSR #26 ;Keep the top half somewhere |
| 277 | MOV R0,R0,LSL #6 ;Shift up so as not to lose |
| 278 | MOV R2,#45 ;Divide by 360 to translate units |
| 279 | BL div_u64x32 ;Now we have Internal Angle Units |
| 280 | MOV R0,R0,LSL #32 - 9 - scalebits% |
| 281 | ;Shift extra revolutions off |
| 282 | |
| 283 | ; --- Now fix it into the first quadrant --- |
| 284 | |
| 285 | AND R3,R0,#&C0000000 ;Get quadrant number in R3 |
| 286 | BIC R0,R0,#&C0000000 ;And angle within quadrant here |
| 287 | TST R3,#&40000000 ;Is it on the left hand side? |
| 288 | RSBNE R0,R0,#&40000000 ;Yes; reflect across the circle |
| 289 | |
| 290 | ; --- Look up the angle in the table --- |
| 291 | |
| 292 | BIC R2,R0,#sin_modmask% ;Get remainder for interp |
| 293 | MOV R0,R0,LSR #sin_shift% ;Convert to a nice index |
| 294 | ADR R14,sin_table ;Find the sin table |
| 295 | ADD R14,R14,R0,LSL #2 ;Find the item to read |
| 296 | LDR R1,[R14,#0] ;Load the lower entry |
| 297 | LDR R0,[R14,#4] ;Load the upper entry too |
| 298 | SUB R0,R0,R1 ;Get the difference out |
| 299 | MUL R0,R2,R0 ;Multiply this by the x offset |
| 300 | ADD R0,R0,#sin_interpoff% ;Add on for rounding |
| 301 | ADD R0,R1,R0,LSR #sin_shift% ;Do linear interp |
| 302 | |
| 303 | ; --- Now postprocess as usual --- |
| 304 | |
| 305 | TST R3,#&80000000 ;Are we down at the bottom there? |
| 306 | RSBNE R0,R0,#0 ;Yes -- negate the sin value |
| 307 | |
| 308 | LDMFD R13!,{R1-R3,PC}^ |
| 309 | |
| 310 | ; --- sin table --- |
| 311 | |
| 312 | .sin_table |
| 313 | ] |
| 314 | |
| 315 | FOR i%=0 TO sin_entries% :REM Go through each index value |
| 316 | index=RAD(i%*90/sin_entries%) :REM Convert to a value in radians |
| 317 | [opt o%:dcd SIN(index)*scale%:] :REM Fill in the sin value nicely |
| 318 | NEXT |
| 319 | |
| 320 | IF bas% THEN |
| 321 | [ opt o% |
| 322 | FNexportAs("arctan","fxp_atan") |
| 323 | FNexportAs("pol","fxp_pol") |
| 324 | FNexportAs("sin","fxp_sin") |
| 325 | FNexportAs("cos","fxp_cos") |
| 326 | ] |
| 327 | ELSE |
| 328 | [ OPT o% |
| 329 | |
| 330 | ; --- divide --- |
| 331 | ; |
| 332 | ; On entry R0 == dividend |
| 333 | ; R1 == divisor |
| 334 | ; |
| 335 | ; On exit R0 == quotient |
| 336 | ; R1 == remainder |
| 337 | ; |
| 338 | ; Use Divides on number by another in a vaguely slow way |
| 339 | |
| 340 | .divide CMP R1,#0 ;Is this a division by zero? |
| 341 | MOVEQ PC,#0 ;Yes -- cause an exception |
| 342 | STMFD R13!,{R2,R14} ;Save some working registers |
| 343 | |
| 344 | ; --- Multiply up the divisor --- |
| 345 | |
| 346 | MOV R14,R1 ;Take a copy of the divisor |
| 347 | CMP R14,R0,LSR #1 ;Is this almost big enough? |
| 348 | .a MOVLS R14,R14,LSL #1 ;No -- shift the divisor up |
| 349 | CMP R14,R0,LSR #1 ;Is this almost big enough? |
| 350 | BLS a ;No -- loop round again |
| 351 | |
| 352 | ; --- Now we can start dividing properly --- |
| 353 | |
| 354 | MOV R2,#0 ;Start the quotient at 0 |
| 355 | .a CMP R0,R14 ;Can we divide here? |
| 356 | SUBCS R0,R0,R14 ;Yes -- subtract the divisor |
| 357 | ADC R2,R2,R2 ;Add with carry nicely |
| 358 | MOV R14,R14,LSR #1 ;Shift divisor back down |
| 359 | CMP R14,R1 ;Have we finished this loop? |
| 360 | BCS a ;And go round again |
| 361 | |
| 362 | ; --- Set up the return values --- |
| 363 | |
| 364 | MOV R1,R0 ;Return remainder in R1 |
| 365 | MOV R0,R2 ;Return quotient in R2 |
| 366 | LDMFD R13!,{R2,PC}^ ;Return to caller |
| 367 | |
| 368 | ; --- div_u64x32 --- |
| 369 | ; |
| 370 | ; On entry R0,R1 = dividend |
| 371 | ; R2 = divisor |
| 372 | ; |
| 373 | ; On exit R0 = quotient |
| 374 | ; R1 = remainder |
| 375 | ; |
| 376 | ; Use Does long division. |
| 377 | |
| 378 | .div_u64x32 STMFD R13!,{R14} |
| 379 | MOV R14,#8 |
| 380 | .a |
| 381 | ] |
| 382 | FOR i% = 1 TO 4 |
| 383 | [ OPT o% |
| 384 | CMP R1,R2 |
| 385 | SUBCS R1,R1,R2 |
| 386 | ADCS R0,R0,R0 |
| 387 | ADC R1,R1,R1 |
| 388 | ] |
| 389 | NEXT |
| 390 | [ OPT o% |
| 391 | SUBS R14,R14,#1 |
| 392 | BGT a |
| 393 | CMP R1,R2 |
| 394 | SUBCS R1,R1,R2 |
| 395 | ADCS R0,R0,R0 |
| 396 | LDMFD R13!,{PC}^ |
| 397 | |
| 398 | .testpol stmfd r13!,{r14} |
| 399 | bl pol |
| 400 | str r0,result |
| 401 | ldmfd r13!,{pc}^ |
| 402 | |
| 403 | .testatn stmfd r13!,{r14} |
| 404 | bl arctan |
| 405 | str r0,result |
| 406 | ldmfd r13!,{pc}^ |
| 407 | |
| 408 | .testsin stmfd r13!,{r14} |
| 409 | bl sin |
| 410 | str r0,result |
| 411 | ldmfd r13!,{pc}^ |
| 412 | |
| 413 | .result dcd 0 |
| 414 | |
| 415 | ] :REM End assembly |
| 416 | ENDIF |
| 417 | |
| 418 | NEXT :REM End current pass |
| 419 | |
| 420 | IF bas% THEN |
| 421 | PROCbas_aofSave |
| 422 | ELSE |
| 423 | |
| 424 | REM --- Test the divide routine --- |
| 425 | |
| 426 | REPEAT :REM Go into a nice loop |
| 427 | INPUT a$, A, B :REM Read the divide arguments |
| 428 | A%=A*scale% |
| 429 | B%=B*scale% |
| 430 | CASE a$ OF |
| 431 | WHEN "sin": CALL testsin: r=SINRAD(A) |
| 432 | WHEN "atn": CALL testatn: r=DEGATN(A) |
| 433 | WHEN "pol": CALL testpol: r=DEGFNpol(A, B) |
| 434 | OTHERWISE: PRINT "wtf?" |
| 435 | ENDCASE |
| 436 | PRINT !result/scale%'r |
| 437 | UNTIL FALSE :REM Keep on looping until bored |
| 438 | |
| 439 | ENDIF |
| 440 | END |
| 441 | |
| 442 | DEF FNpol(x,y) |
| 443 | LOCAL a |
| 444 | |
| 445 | IF x=0 THEN a=PI/2 ELSE a=ATN(ABS(y/x)) |
| 446 | IF y<0 THEN |
| 447 | IF x<0 THEN a=PI+a ELSE a=2*PI-a |
| 448 | ELSE |
| 449 | IF x<0 THEN a=PI-a |
| 450 | ENDIF |
| 451 | =a |