--- /dev/null
+REM
+REM fixedPt.bs
+REM
+REM Table-based trig functions
+REM
+REM © 1995-1998 Straylight
+REM
+
+REM ----- Licensing note ----------------------------------------------------
+REM
+REM This file is part of Straylight's Sapphire library.
+REM
+REM Sapphire is free software; you can redistribute it and/or modify
+REM it under the terms of the GNU General Public License as published by
+REM the Free Software Foundation; either version 2, or (at your option)
+REM any later version
+REM
+REM Sapphire is distributed in the hope that it will be useful,
+REM but WITHOUT ANY WARRANTY; without even the implied warranty of
+REM MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+REM GNU General Public License for more details.
+REM
+REM You should have received a copy of the GNU General Public License
+REM along with Sapphire. If not, write to the Free Software Foundation,
+REM 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+
+REM --- How the table works ---
+REM
+REM We represent a number in 16.16 fixed point. We set up a table of
+REM arctan values for values of x between 0 and 1. All the others may be
+REM calculated from this, hopefully. Certainly, this is enough for
+REM calculating polar angles, which is the really interesting bit.
+REM
+REM For niceness purposes, the values are actually stored in degrees, rather
+REM than radians. Division by 360 isn't a major difficulty, so it's not
+REM hard to convert back again.
+
+REM --- Set up some variables ---
+
+ON ERROR ERROR 0,REPORT$+" ["+STR$(ERL)+"]"
+bas%=TRUE :REM Generate BAS code
+bas%=bas%
+
+REM --- BAS library ---
+
+IF bas% THEN
+ LIBRARY "libs:bas"
+ PROCbas_init
+ENDIF
+
+REM --- A little bit of test code ---
+
+IF bas% THEN
+ PROCbas_aofInit(&10000)
+ o_base%=4
+ o_lim%=6
+ELSE
+ DIM code% 10240
+ o_base%=0
+ o_lim%=2
+ENDIF
+
+REM --- General parameters ---
+
+scalebits% = 16 :REM Avoiding tweaking this: it's part of the interface
+scale% = 1 << scalebits%
+
+REM --- Parameters for the tan table --
+
+atn_tblbits% = 7 :REM TWEAK ME!
+atn_entries% = 1 << atn_tblbits%
+atn_shift% = scalebits% - atn_tblbits%
+atn_revshift% = 32 - atn_shift%
+atn_interpoff% = 1 << (atn_shift% - 1)
+
+REM --- Parameters for the sin table ---
+
+sin_tblbits% = 6 :REM TWEAK ME! sin_tblbits may be at most 8
+sin_entries% = 1 << sin_tblbits%
+sin_shift% = 30 - sin_tblbits%
+sin_modmask% = (sin_entries% - 1) << sin_shift%
+sin_interpoff% = 1 << (sin_shift% - 1)
+
+REM --- Start the assembly ---
+
+FOR o%=o_base% TO o_lim% STEP 2 :REM Standard two-pass assembly loop
+ IF bas%=0 THEN P%=code% :REM Start generating code in buffer
+
+ IF bas% THEN
+ [ OPT o%
+ FNpass
+
+ FNarea("Sapphire$$Code","CODE,READONLY")
+
+ FNimport("div_unsigned")
+ FNimport("div_u64x32")
+ ]
+ ENDIF
+
+ [ OPT o% :REM Start assembly
+
+ ; --- arctan ---
+ ;
+ ; On entry R0 == x, in 16.16 fixed point form
+ ;
+ ; On exit R0 == arctan x, in degrees, in 16.16 fixed point
+ ;
+ ; Use Calculates arctan x, hopefully fairly swiftly. The
+ ; accuracy of the result is open to doubt, although
+ ; it's usually good to about 3 significant figures.
+ ; It uses a small lookup table and linear interpolation
+ ; to calculate the result.
+
+ .arctan STMFD R13!,{R1-R3,R14} ;Save some work registers
+
+ ; --- Set some initial things up ---
+
+ MOV R3,#0 ;Clear out my flags register
+
+ ; --- Now sort out the argument ---
+
+ CMP R0,#0 ;Is the argument negative?
+ ORRLT R3,R3,#1 ;Yes -- set the `negative' bit
+ RSBLT R0,R0,#0 ;And make the argument positive
+
+ CMP R0,#scale% ;Is the argument bigger than 1?
+ BLE atn_skip ;No; skip ahead a bit
+
+ ORR R3,R3,#2 ;Set the `reciprocal' bit
+ ]
+ IF scalebits% < 16 THEN
+ [ OPT o%
+ MOV R1,R0 ;Stand by to do some division
+ MOV R0,#1 << (scalebits% * 2)
+ BL div_unsigned
+ ]
+ ELSE
+ [ OPT o%
+ MOV R2,R0 ;Stand by to do some division
+ MOV R0,#0
+ MOV R1,#1 << (scalebits% * 2 - 32)
+ BL div_u64x32
+ ]
+ ENDIF
+ [ OPT o%
+
+ ; --- Look up the arctan value ---
+
+ .atn_skip ADR R14,atn_table ;Load the address of the table
+ MOV R1,R0,LSL #atn_revshift% ;Get bottom bits in R1
+ MOV R1,R1,LSR #atn_revshift% ;Shift back to bit 0
+ MOV R0,R0,LSR #atn_shift% ;Get the table index
+ ADD R14,R14,R0,LSL #2 ;Get the address of the item
+ LDR R2,[R14,#0] ;Locate the lower entry and load
+ LDR R0,[R14,#4] ;Locate the upper entry and load
+ SUB R0,R0,R2 ;Get the difference between them
+ MUL R0,R1,R0 ;Multiply this by the x offset
+ ADD R0,R0,#atn_interpoff% ;Round to nearest
+ ADD R0,R2,R0,LSR #atn_shift% ;Do interpolation
+
+ ; --- Now postprocess this result nicely ---
+
+ TST R3,#2 ;Did I have to reciprocal it?
+ RSBNE R0,R0,#90*scale% ;Yes -- work out the correct one
+ TST R3,#1 ;Did I have to negate the value?
+ RSBNE R0,R0,#0 ;Yes -- negate the result
+
+ LDMFD R13!,{R1-R3,PC}^ ;Return to caller
+
+ ; --- pol ---
+ ;
+ ; On entry R0 == x coordinate
+ ; R1 == y coordinate
+ ;
+ ; On exit R0 == angle in degrees, in 16.16 form
+ ;
+ ; Use Calculates the angle a vector makes with the +ve x axis.
+ ; The angle is given in degrees, rather than radians,
+ ; although this isn't really overly significant, it just
+ ; makes it slightly easier to work with, because it's
+ ; bigger.
+ ;
+ ; This routine uses the arctan table and linear
+ ; interpolation, so it's fairly quick, but the accuracy
+ ; of its results is questionable.
+
+ .pol STMFD R13!,{R1-R3,R14} ;Save some registers away
+ MOV R3,#0 ;Clear out my status flags
+
+ ; --- Preprocess the arguments ---
+
+ CMP R0,#0 ;Is x less than zero?
+ RSBLT R0,R0,#0 ;Yes -- make it positive
+ ORRLT R3,R3,#1 ;And remember that we did this
+
+ CMP R1,#0 ;Is y less than zero?
+ RSBLT R1,R1,#0 ;Yes -- make it positive
+ ORRLT R3,R3,#2 ;And remember we did this
+
+ CMP R0,R1 ;Is x bigger than y?
+ EORGT R0,R0,R1 ;Yes -- then swap them round
+ EORGT R1,R0,R1
+ EORGT R0,R0,R1
+ ORRGT R3,R3,#4 ;And remember we did this
+
+ ; --- Handle silly case where point is at (0,0) ---
+
+ CMP R1,#0 ;Are we about to divide by 0?
+ LDMEQFD R13!,{R1-R3,PC}^ ;Yes -- return now -- R0 is 0
+
+ ; --- Now work out arctan(R0/R1) ---
+
+ MOV R2,R1 ;Get the divisor
+ MOV R1,R0,LSR #32 - scalebits% ;Sort out dividend
+ MOV R0,R0,LSL #scalebits%
+ BL div_u64x32 ;Work out the ratio nicely
+
+ ADR R14,atn_table ;Load the address of the table
+ MOV R1,R0,LSL #atn_revshift% ;Get bottom bits in R1
+ MOV R1,R1,LSR #atn_revshift% ;Shift back to bit 0
+ MOV R0,R0,LSR #atn_shift% ;Get correct table index
+ ADD R14,R14,R0,LSL #2 ;Get the address of the item
+ LDR R2,[R14,#0] ;Locate the lower entry and load
+ LDR R0,[R14,#4] ;Locate the upper entry and load
+ SUB R0,R0,R2 ;Get the difference between them
+ MUL R0,R1,R0 ;Multiply this by the x offset
+ ADD R0,R0,#atn_interpoff% ;Just round to nearest
+ ADD R0,R2,R0,LSR #atn_shift% ;And add to the base
+
+ ; --- arctan is now in R0 -- process it for angle ---
+
+ TST R3,#4 ;Did we swap x and y round?
+ RSBEQ R0,R0,#90*scale% ;No -- compensate for this
+ TST R3,#1 ;Was x negative?
+ RSBNE R0,R0,#180*scale% ;Yes -- push it to left side
+ TST R3,#2 ;Was y negative?
+ RSBNE R0,R0,#360*scale% ;Yes -- push it to bottom
+
+ LDMFD R13!,{R1-R3,PC}^ ;Return to caller
+
+ ; --- arctan table ---
+
+ .atn_table
+ ]
+
+ FOR i%=0 TO atn_entries% :REM Go through each index value
+ a=DEG ATN(i%/atn_entries%) :REM Calculate the arctan value
+ [opt o%:dcd a*scale%:] :REM Store it in the table nicely
+ NEXT
+
+ [ OPT o%
+
+ ; --- sin ---
+ ;
+ ; On entry R0 == angle in degrees, in 16.16 form
+ ;
+ ; On exit R0 == sin of angle, in 16.16 form
+ ;
+ ; Use Calculates a sin of an angle with a degree of swiftness
+ ; and a lot less accuracy.
+
+ .cos RSB R0,R0,#90*scale%
+
+ .sin STMFD R13!,{R1-R3,R14} ;Save a job load of registers
+
+ ; --- How it all works ---
+ ;
+ ; Having angles in degrees imposes inconvenient units
+ ; on use, forcing us to do all sorts of horrible things
+ ; with division by 360.
+ ;
+ ; Hence, we translate the degree units into nice things
+ ; which represent a right angle as a power of two,
+ ; say 2^16.
+
+ MOV R1,R0,LSR #26 ;Keep the top half somewhere
+ MOV R0,R0,LSL #6 ;Shift up so as not to lose
+ MOV R2,#45 ;Divide by 360 to translate units
+ BL div_u64x32 ;Now we have Internal Angle Units
+ MOV R0,R0,LSL #32 - 9 - scalebits%
+ ;Shift extra revolutions off
+
+ ; --- Now fix it into the first quadrant ---
+
+ AND R3,R0,#&C0000000 ;Get quadrant number in R3
+ BIC R0,R0,#&C0000000 ;And angle within quadrant here
+ TST R3,#&40000000 ;Is it on the left hand side?
+ RSBNE R0,R0,#&40000000 ;Yes; reflect across the circle
+
+ ; --- Look up the angle in the table ---
+
+ BIC R2,R0,#sin_modmask% ;Get remainder for interp
+ MOV R0,R0,LSR #sin_shift% ;Convert to a nice index
+ ADR R14,sin_table ;Find the sin table
+ ADD R14,R14,R0,LSL #2 ;Find the item to read
+ LDR R1,[R14,#0] ;Load the lower entry
+ LDR R0,[R14,#4] ;Load the upper entry too
+ SUB R0,R0,R1 ;Get the difference out
+ MUL R0,R2,R0 ;Multiply this by the x offset
+ ADD R0,R0,#sin_interpoff% ;Add on for rounding
+ ADD R0,R1,R0,LSR #sin_shift% ;Do linear interp
+
+ ; --- Now postprocess as usual ---
+
+ TST R3,#&80000000 ;Are we down at the bottom there?
+ RSBNE R0,R0,#0 ;Yes -- negate the sin value
+
+ LDMFD R13!,{R1-R3,PC}^
+
+ ; --- sin table ---
+
+ .sin_table
+ ]
+
+ FOR i%=0 TO sin_entries% :REM Go through each index value
+ index=RAD(i%*90/sin_entries%) :REM Convert to a value in radians
+ [opt o%:dcd SIN(index)*scale%:] :REM Fill in the sin value nicely
+ NEXT
+
+ IF bas% THEN
+ [ opt o%
+ FNexportAs("arctan","fxp_atan")
+ FNexportAs("pol","fxp_pol")
+ FNexportAs("sin","fxp_sin")
+ FNexportAs("cos","fxp_cos")
+ ]
+ ELSE
+ [ OPT o%
+
+ ; --- divide ---
+ ;
+ ; On entry R0 == dividend
+ ; R1 == divisor
+ ;
+ ; On exit R0 == quotient
+ ; R1 == remainder
+ ;
+ ; Use Divides on number by another in a vaguely slow way
+
+ .divide CMP R1,#0 ;Is this a division by zero?
+ MOVEQ PC,#0 ;Yes -- cause an exception
+ STMFD R13!,{R2,R14} ;Save some working registers
+
+ ; --- Multiply up the divisor ---
+
+ MOV R14,R1 ;Take a copy of the divisor
+ CMP R14,R0,LSR #1 ;Is this almost big enough?
+ .a MOVLS R14,R14,LSL #1 ;No -- shift the divisor up
+ CMP R14,R0,LSR #1 ;Is this almost big enough?
+ BLS a ;No -- loop round again
+
+ ; --- Now we can start dividing properly ---
+
+ MOV R2,#0 ;Start the quotient at 0
+ .a CMP R0,R14 ;Can we divide here?
+ SUBCS R0,R0,R14 ;Yes -- subtract the divisor
+ ADC R2,R2,R2 ;Add with carry nicely
+ MOV R14,R14,LSR #1 ;Shift divisor back down
+ CMP R14,R1 ;Have we finished this loop?
+ BCS a ;And go round again
+
+ ; --- Set up the return values ---
+
+ MOV R1,R0 ;Return remainder in R1
+ MOV R0,R2 ;Return quotient in R2
+ LDMFD R13!,{R2,PC}^ ;Return to caller
+
+ ; --- div_u64x32 ---
+ ;
+ ; On entry R0,R1 = dividend
+ ; R2 = divisor
+ ;
+ ; On exit R0 = quotient
+ ; R1 = remainder
+ ;
+ ; Use Does long division.
+
+ .div_u64x32 STMFD R13!,{R14}
+ MOV R14,#8
+ .a
+ ]
+ FOR i% = 1 TO 4
+ [ OPT o%
+ CMP R1,R2
+ SUBCS R1,R1,R2
+ ADCS R0,R0,R0
+ ADC R1,R1,R1
+ ]
+ NEXT
+ [ OPT o%
+ SUBS R14,R14,#1
+ BGT a
+ CMP R1,R2
+ SUBCS R1,R1,R2
+ ADCS R0,R0,R0
+ LDMFD R13!,{PC}^
+
+ .testpol stmfd r13!,{r14}
+ bl pol
+ str r0,result
+ ldmfd r13!,{pc}^
+
+ .testatn stmfd r13!,{r14}
+ bl arctan
+ str r0,result
+ ldmfd r13!,{pc}^
+
+ .testsin stmfd r13!,{r14}
+ bl sin
+ str r0,result
+ ldmfd r13!,{pc}^
+
+ .result dcd 0
+
+ ] :REM End assembly
+ ENDIF
+
+NEXT :REM End current pass
+
+IF bas% THEN
+ PROCbas_aofSave
+ELSE
+
+ REM --- Test the divide routine ---
+
+ REPEAT :REM Go into a nice loop
+ INPUT a$, A, B :REM Read the divide arguments
+ A%=A*scale%
+ B%=B*scale%
+ CASE a$ OF
+ WHEN "sin": CALL testsin: r=SINRAD(A)
+ WHEN "atn": CALL testatn: r=DEGATN(A)
+ WHEN "pol": CALL testpol: r=DEGFNpol(A, B)
+ OTHERWISE: PRINT "wtf?"
+ ENDCASE
+ PRINT !result/scale%'r
+ UNTIL FALSE :REM Keep on looping until bored
+
+ENDIF
+END
+
+DEF FNpol(x,y)
+LOCAL a
+
+IF x=0 THEN a=PI/2 ELSE a=ATN(ABS(y/x))
+IF y<0 THEN
+ IF x<0 THEN a=PI+a ELSE a=2*PI-a
+ELSE
+ IF x<0 THEN a=PI-a
+ENDIF
+=a
~mdw / ssr / blobdiff