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