X-Git-Url: https://git.distorted.org.uk/~mdw/ssr/blobdiff_plain/a3da4c116d4216fdec125d687dfc347e343a8b28..c1b567d833a004bb3d978f1f7c99f42cefa7845c:/StraySrc/Libraries/Sapphire/bs/fixedPt.bas diff --git a/StraySrc/Libraries/Sapphire/bs/fixedPt.bas b/StraySrc/Libraries/Sapphire/bs/fixedPt.bas new file mode 100644 index 0000000..29917b1 --- /dev/null +++ b/StraySrc/Libraries/Sapphire/bs/fixedPt.bas @@ -0,0 +1,451 @@ +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