chiark - git - mdw - ssr/blob - StraySrc/Libraries/Sapphire/bs/fixedPt.bas

   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