perm filename ARITH.PAL[U,VDS] blob
sn#300589 filedate 1977-08-07 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00009 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 .TITLE ARITH
C00005 00003 "ATAN2" - COMPUTES THE ARC-TANGENT WITH TWO ARGUMENTS
C00008 00004 "SQRT" - COMPUTES THE SQUARE ROOT OF A DOUBLE PRECISION INTEGER
C00010 00005 "MATMUL" - SUBR. TO MULTIPLY TWO TRANSFORMS TOGETHER
C00012 00006 "MUL3X3" - COMPUTES THE RIGHT 3 X 3 OF A TRANSFORM MATRIX
C00015 00007 .IFNZ 0 "MUL3X3" - COMPUTES THE RIGHT 3 X 3 OF A TRANSFORM MATRIX
C00019 00008 "CROSS" - COMPUTES THE CROSS PRODUCT OF TWO VECTORS
C00021 00009 TABLES OF SINE/COSINE AND ARC-TANGENT
C00025 ENDMK
C⊗;
.TITLE ARITH
;"SNCOS" - SINE/COSINE FUNCTION USING TABLE LOOKUP
;THIS PROGRAM CALCULATES BOTH THE SINE AND THE COSINE OF A ANGLE USING
;A TABLE LOOP UP PROCEDURE. THE IMPLEMENTED APPROXIMATION EQUATIONS
;ARE AS FOLLOWS:
; SIN(X) = SIN(A) + (B/I)*[SIN(A+I)-SIN(A)]
; COS(X) = COS(A) + (B/I)*[COS(A+I)-COS(A)]
; WHERE
; I = 90/128 DEGREES
; A = INTEGER(X*128/90)
; B = REMAINDER(X*128/90)
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV ANGLE,R0
; JSR PC,SNCOS
; MOV R0,SIN
; MOV R1,COS
;
;FOR ANGLES, 180 DEG = '40000. FOR THE RESULTS, 1 = '40000.
;ALL OF THE VALUES RETURNED BY THIS RTN. ARE ACCURATE TO
;WITHIN + OR - ONE COUNT.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
SNCOS: MOV R2,-(SP) ;SAVE THE REGISTERS
MOV R3,-(SP)
MOV R0,-(SP) ;SAVE ANGLE
BIT #20000,R0 ;SHIFT ANGLE TO QUADRANT 1
BEQ .+4
NEG R0
BIC #140000,R0
CLR R1 ;PUT A IN R0, B/I IN R1
ASHC @#KM6,R0
ROR R1
ASL R0
MOV R0,-(SP) ;SAVE A FOR COMPUTATION OF COSINE
MOV R1,-(SP) ;SAVE B/I
MOV SINE+2(R0),R2 ;SIN(A+I)
MOV SINE(R0),R0 ;SIN(A)
SUB R0,R2
ASL R2
MUL R1,R2 ;(B/I)*[SIN(A+1)-SIN(A)]
ADD R2,R0 ;NOW HAVE ABS(SIN(X))
INC R0 ;SCALE AND ROUND
CLC
ROR R0
BIT #40000,4(SP) ;SIN ← -SIN IF X IN QUAD 3 OR 4
BEQ .+4
NEG R0
MOV (SP)+,R3 ;GET B/I
MOV (SP)+,R1 ;GET A
NEG R1 ;ENTER TABLE FROM OPPOSITE END
MOV COSINE-2(R1),R2 ;COS(A+I)
MOV COSINE(R1),R1 ;COS(A)
SUB R1,R2
ASL R2
MUL R3,R2 ;(B/I)*[COS(A+I)-COS(A)]
ADD R2,R1 ;NOW HAVE ABS(COS(X))
INC R1 ;SCALE AND ROUND
CLC
ROR R1
ADD #20000,(SP) ;COS ← -COS IF QUADRANT 2 OR 3
BIT #40000,(SP)+
BEQ .+4
NEG R1
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;END OF "SNCOS"
;"ATAN2" - COMPUTES THE ARC-TANGENT WITH TWO ARGUMENTS
;COMPUTES THE ARC-TANGENT OF A/B USING A TABLE LGOK UP SCHEME. SINCE
;TWO ARGUMENTS ARE USED SINGULARITIES ARE AVOIDED AT MULTIPLES OF PI/2
;AND THERE IS NO AMBIGUITY CONCERNING QUADRANTS. THE ARGUMENT A MUST
;BE LOADED INTO R0 AND B INTO R1 BEFORE CALLING ATAN2. AFTER
;EXECUTION, ATAN RETURNS THE ARC-TANGENT IN R0.
;THIS ROUTINE RETURNS AN ANSWER WHICH IS CORRECT TO WITHIN + OR -
;ONE COUNT.
FLAG==%4
RISE==1 ;RISE ISPOSITIVE
RUN==2 ;RUN POSITIVE
CMPA==4 ;COMPLEMENTARY ANGLE
ATAN2: ;(R0=@RISE, R1=@RUN)
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
CLR FLAG
TST R0 ;RISE
BLT RISNEG
BGT RISPOS
TST R1 ;RISE IS ZERO
BPL .+6
NA: BIS #140000,R0 ;-PI
JRET: MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
RISPOS: NEG R0 ;MAKE NUM NEG
BIS #RISE,FLAG
RISNEG: TST R1 ;RUN
BLT RUNNEG
BGT RUNPOS
MOV #20000,R0 ;PI/2
BIT #RISE,FLAG
BEQ NA
BR JRET
RUNPOS: NEG R1 ;MAKE DEMON NEG
BIS #RUN,FLAG
RUNNEG: CMP R0,R1
BMI MREV ;MUST EXCHANGE
BNE AOK
MOV #10000,R0 ;PI/4
BR FQ
MREV: MOV R0,R2 ;COMPUTE COMPLEMENT B/A
MOV R1,R0
BIS #CMPA,FLAG
BR RDY
AOK: MOV R1,R2 ;COMPUTE A/B
RDY: CLR R1 ;GET TABLE INDEX
ASHC @#KM1,R0
DIV R2,R0
CLR R1
ASHC @#KM8,R0
ROR R1 ;THIS IS THE INTERPOLATION FACTOR
ASL R0
MOV ARCTAN+2(R0),R2 ;GET ATAN ON EITHER SIDE OF THETA
MOV ARCTAN(R0),R0
SUB R0,R2 ;INTERPOLATE
ASL R2
MUL R1,R2
ADD R2,R0
INC R0
ASR R0
FQ: BIT #CMPA,FLAG ;THETA = 90-THETA IF COMPLEMENT
BEQ NCMP
SUB #20000,R0
NEG R0
NCMP: BIT #RUN,FLAG ;THETA = 180-THETA IF COS <0
BNE RPOS
BIS #140000,R0
NEG R0
RPOS: BIT #RISE,FLAG ;THETA = -THETA IF SIN < 0
BNE JRET
NEG R0
BR JRET
;"SQRT" - COMPUTES THE SQUARE ROOT OF A DOUBLE PRECISION INTEGER
;THE NUMBER IS ASSUMED TO BE STORED IN REGISTERS R0 AND R1. THIS
;ROUTINE USES A LINEAR APPROXIMATION TO THE SQUARE ROOT OF THE
;NUMBER AND PERFORMS ONE ITERATION TO INCREASE THE ACCURACY TO
;ALMOST 16 BITS. AFTER EXECUTION, THE RESULT IS LEFT IN R0.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
SQRT: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
TST R0 ;RETURN 0 IF ARGUMENT NEGATIVE
BGE SQRT1
CLR R0
CLR R1
SQRT1: BGT SQRT2 ;EXIT IF ARGUMENT JUST ZERO
TST R1
BEQ SQRDNE
SQRT2: CLR R4
SQRT3: BIT #170000,R0 ;NORMALIZE NUMBER BETWEEN 1/4 AND 1
BNE SQRT4
ASHC @#K2,R0 ;ADJUST BY 2↑2
SOB R4,SQRT3
SQRT4: MOV R0,R2
MOV R1,R3
ASHC @#KM2,R2
BIT #20000,R0
BNE HALF
MUL @#K64XX,R0 ;64000
ADD #4660,R0
BR ITER
HALF: MUL @#K45XX,R0 ;45000
ADD #6600,R0
ITER: ASL R0 ;ARG/GUESS
DIV R0,R2
ADD R2,R0 ;ARG/GUESS + GUESS
NEG R4
BEQ SQRDNE
ASR R0
BIC #100000,R0
DEC R4
BEQ SQRDNE
SQRT5: ASR R0
SOB R4,SQRT5
SQRDNE: MOV (SP)+,R4 ;RESTORE THE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;END OF "SQRT"
;"MATMUL" - SUBR. TO MULTIPLY TWO TRANSFORMS TOGETHER
;THE TRANSFORM "T1" IS MULTIPLIED BY "T2" AND THE RESULT IS STORED IN
;"T0". IT IS ASSUMED THAT THE TRANSFORMS ARE STORED AS 3 X 4
;COLUMN MATRICES. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #T0,R0 ;LOAD ADDRESS OF TRANSFORMS
; MOV #T1,R1
; MOV #T2,R2
; JSR PC,MATMUL ;T0 ← T1 X T2
;
;THE SAME TRANSFORM CAN BE GIVEN AS ANY TWO OF THE REQUIRED ARGUMENTS.
;IN FACT THE SAME TRANSFORM CAN BE GIVEN FOR ALL THREE ARGUMENTS.
;REGISTERS USED:
; R0,R1,R2 PASS ARGUMENTS AND R0 IS NOT MODIFIED
MATMUL: MOV R1,-(SP)
MOV R0,-(SP)
JSR PC,MUL3X3 ;COMPUTE THE RIGHT 3 X 3 FIRST
MOV (SP),R2 ;COMPUTE THE FIRST COLUMN
MOV R2,R0
ADD #6,R0
MOV R0,R1
ADD #6,R1
JSR PC,CROSS ;1ST COL ← 2ND COL X 3RD COL
MOV (SP)+,R0 ;RESTORE POINTERS
MOV (SP)+,R1
ADD 22(R1),22(R0) ;ADD IN THE CONTRIBUTION OF T44
ADD 24(R1),24(R0)
ADD 26(R1),26(R0)
RTS PC
;END OF "MATMUL"
;"MUL3X3" - COMPUTES THE RIGHT 3 X 3 OF A TRANSFORM MATRIX
;MATRIX "T2" IS MULTIPLIED BY "T3" AND THE RESULT IS STORED IN "T1".
;THE MATRICES MUST BE STORED BY COLUMNS. EACH COLUMN MUST ONLY BE
;THREE ROWS DEEP. ONLY THE RIGHT 3 X 3 OF "T1" IS COMPUTED BY
;THIS ROUTINE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #T1,R0
; MOV #T2,R1
; MOV #T3,R2
; JSR PC,MUL3X3 ;T1 ← T2 X T3
;
;THIS ROUTINE NEVER RETURNS AN ERROR MESSAGE
;REGISTERS USED:
; R0, R1, R2 PASS ARGUMENTS AND ARE ALTERED
MUL3X3: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP)
ADD #6,R2 ;POINT TO SECOND COLUMN OF T3
ADD #14,R0 ;POINT TO 2ND COL, BOT ROW OF T1-1
MOV #3,R3 ;PUSH LEFT 3 X 3 OF T2 ONTO STK
MUL3LP: MOV R3,-(SP) ;MARK THE LAYERS OF THE STK
MOV 14(R1),-(SP) ;PUSH A ROW OF T2
MOV 6(R1),-(SP)
MOV (R1)+,-(SP)
SOB R3,MUL3LP
MOV R2,R1
ROWLP: JSR PC,MULCOL ;COMPUTE 1ST COL OF CURRENT ROW
MOV R2,-(R0) ;SAVE VALUE
JSR PC,MULCOL ;2ND COLUMN
MOV R2,6(R0)
JSR PC,MULCOL ;3RD COLUMN
MOV R2,14(R0)
SUB #22,R1 ;RESTORE PTR TO T3
ADD #6,SP ;CLEAR OFF A ROW OF T2
CMP #3,(SP)+ ;REPEAT FOR THREE ROWS OF T1
BNE ROWLP
MOV (SP)+,R3 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC
MULCOL: MOV 2(SP),R2 ;A(1)*B(1)
MUL (R1)+,R2
MOV 4(SP),R4 ;+A(2)*B(2)
MUL (R1)+,R4
ADD R5,R3
ADC R2
ADD R4,R2
MOV 6(SP),R4 ;+A(3)*B(3)
MUL (R1)+,R4
ADD R5,R3
ADC R2
ADD R4,R2
ASHC @#K2,R2 ;NORMALIZE AND ROUND
ROL R3
ADC R2
RTS PC
;END OF "MUL3X3"
.IFNZ 0 ;"MUL3X3" - COMPUTES THE RIGHT 3 X 3 OF A TRANSFORM MATRIX
;MATRIX "T1" IS MULTIPLIED BY "T2" AND THE RESULT IS STORED IN "T0".
;THE MATRICES MUST BE STORED BY COLUMNS. EACH COLUMN MUST ONLY BE
;THREE ROWS DEEP. ONLY THE RIGHT 3 X 3 OF "T0" IS COMPUTED BY
;THIS ROUTINE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #T0,R0
; MOV #T1,R1
; MOV #T2,R2
; JSR PC,MUL3X3 ;T0 ← T1 X T2
;
;THIS ROUTINE NEVER RETURNS AN ERROR MESSAGE
;REGISTERS USED:
; R0, R1, R2 PASS ARGUMENTS AND ARE ALTERED
MUL3XX: MOV R3,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R5,-(SP)
ADD #6,R2 ;POINT TO SECOND COLUMN OF T2
ADD #6,R0 ; " " " " OF T0
ADD #26,R1 ;POINT TO T34 OF T1
MOV (R1),-(SP) ;PUSH LAST COLUMN OF T1
MOV -(R1),-(SP)
MOV -(R1),-(SP)
MOV #3,R3 ;PUSH LEFT 3X3 OF T1 BY ROWS
1$: MOV -(R1),-(SP)
MOV -6(R1),-(SP)
MOV -14(R1),-(SP)
SOB R3,1$
MOV SP,R1
JSR PC,MULC2 ;COMPUTE 2ND COL T0
MOV SP,R1
JSR PC,MULC2 ;3RD COLUMN
MOV SP,R1
JSR PC,MULC2 ;4TH COLUMN
MOV R1,SP ;CLEAR T1 3X3 OFF STACK
SUB #6,R0 ;ADD IN 4TH COL CONTRIBUTION OF R1
ADD (SP)+,(R0)+
ADD (SP)+,(R0)+
ADD (SP)+,(R0)
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
RTS PC
MULC2: MOV #3,R3
BR .+4
1$: CMP -(R2),-(R2)
MOV (R1)+,R4 ;A(1)*B(1)
MUL (R2)+,R4
MOV R4,-(SP)
MOV R5,-(SP)
MOV (R1)+,R4
MUL (R2)+,R4
ADD R5,(SP)
ADC 2(SP)
ADD R4,2(SP)
MOV (R1)+,R4
MUL (R2),R4
ADD (SP)+,R5
ADC R4
ADD (SP)+,R4
ASHC @#K2,R4
ROL R5
ADC R4
MOV R4,(R0)+
SOB R3,1$
TST (R2)+
RTS PC
;END OF "MUL3X3"
MATMU2: MOV R0,-(SP)
JSR PC,MUL3X3 ;COMPUTE THE RIGHT 3 X 3 FIRST
MOV (SP),R2 ;COMPUTE THE FIRST COLUMN
MOV R2,R0
ADD #6,R0
MOV R0,R1
ADD #6,R1
JSR PC,CROSS ;1ST COL ← 2ND COL X 3RD COL
MOV (SP)+,R0 ;RESTORE POINTERS
RTS PC
;END OF "MATMUL"
MATMU2: MOV R4,-(SP)
MOV R3,-(SP)
MOV R1,-(SP)
MOV R0,-(SP)
JSR PC,MUL3X3
MOV (SP),R4
JSR PC,CROSS3
MOV (SP)+,R0
MOV (SP)+,R1
MOV (SP)+,R3
MOV (SP)+,R4
ADD 22(R1),22(R0)
ADD 24(R1),24(R0)
ADD 26(R1),26(R0)
RTS PC
CROSS2: MOV 10(R4),R0
MUL 20(R4),R0
MOV 12(R4),R2
MUL 16(R4),R2
SUB R3,R1
SBC R0
SUB R2,R0
RND0
MOV R0,(R4)+
MOV 10(R4),R0
MUL 12(R4),R0
MOV 4(R4),R2
MUL 16(R4),R2
SUB R3,R1
SBC R0
SUB R2,R0
RND0
MOV R0,(R4)+
MOV 2(R4),R0
MUL 12(R4),R0
MOV 4(R4),R2
MUL 10(R4),R2
SUB R3,R1
SBC R0
SUB R2,R0
RND0
MOV R0,(R4)
RTS PC
.ENDC
;"CROSS" - COMPUTES THE CROSS PRODUCT OF TWO VECTORS
;COMPUTES THE CROSS PRODUCT OF "B x C" AND RETURNS THE RESULT IN "A".
;A,B, AND C MUST BE ARRAYS CONTAINING THREE ELEMENTS. A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #B,R0
; MOV #C,R1
; MOV #A,R2
; JSR PC,CROSS
;REGISTERS USED:
; R0,R1,R2 PASS ARGUMENTS AND ARE ALTERED
CROSS: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP) ;A(1)
TST (R2)+
MOV R2,-(SP) ;A(2)
TST (R2)+
MOV R2,-(SP) ;A(3);
MOV (R0),R2 ;A(3) ← B(1)C(2)-B(2)C(1)
MUL 2(R1),R2
MOV 2(R0),R4
MUL (R1),R4
JSR PC,GETAA
MOV R2,@(SP)+
MOV 4(R0),R2 ;A(2) ← B(3)C(1) - B(1)C(3)
MUL (R1),R2
MOV (R0),R4
MUL 4(R1),R4
JSR PC,GETAA
MOV R2,@(SP)+
MOV 2(R0),R2 ;A(1) ← B(2)C(3) - B(3)C(2)
MUL 4(R1),R2
MOV 4(R0),R4
MUL 2(R1),R4
JSR PC,GETAA
MOV R2,@(SP)+
MOV (SP)+,R3 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC
GETAA: SUB R5,R3
SBC R2
SUB R4,R2
ASHC @#K2,R2
ROL R3
ADC R2
RTS PC
;END OF "CROSS"
;TABLES OF SINE/COSINE AND ARC-TANGENT
SINE: .WORD 0, 622, 1444, 2266, 3110
.WORD 3731, 4553, 5373, 6214, 7034
.WORD 7653, 10472, 11310, 12125, 12742
.WORD 13556, 14371, 15203, 16014, 16623
.WORD 17432, 20237, 21044, 21647, 22450
.WORD 23250, 24047, 24644, 25437, 26231
.WORD 27021, 27607, 30374, 31156, 31737
.WORD 32516, 33272, 34045, 34615, 35363
.WORD 36127, 36670, 37427, 40164, 40716
.WORD 41446, 42173, 42715, 43435, 44152
.WORD 44664, 45373, 46100, 46601, 47300
.WORD 47773, 50464, 51151, 51633, 52312
.WORD 52766, 53436, 54103, 54544, 55202
.WORD 55635, 56264, 56710, 57327, 57744
.WORD 60354, 60761, 61362, 61757, 62351
.WORD 62736, 63320, 63675, 64247, 64614
.WORD 65156, 65513, 66044, 66371, 66712
.WORD 67227, 67537, 70043, 70343, 70636
.WORD 71125, 71410, 71666, 72140, 72405
.WORD 72646, 73102, 73331, 73554, 73773
.WORD 74205, 74412, 74612, 75006, 75175
.WORD 75357, 75535, 75706, 76052, 76211
.WORD 76344, 76472, 76612, 76726, 77036
.WORD 77140, 77235, 77326, 77412, 77470
.WORD 77542, 77607, 77647, 77702, 77731
.WORD 77752, 77766, 77776
COSINE: .WORD 100000
ARCTAN: .WORD 0 ,121 ,243 ,364 ,506
.WORD 627 ,751 ,1072 ,1213 ,1334
.WORD 1455 ,1576 ,1717 ,2040 ,2160
.WORD 2301 ,2421 ,2541 ,2661 ,3001
.WORD 3121 ,3240 ,3357 ,3476 ,3615
.WORD 3734 ,4052 ,4170 ,4306 ,4424
.WORD 4541 ,4656 ,4773 ,5110 ,5224
.WORD 5340 ,5454 ,5567 ,5702 ,6015
.WORD 6127 ,6241 ,6353 ,6464 ,6575
.WORD 6706 ,7017 ,7126 ,7236 ,7345
.WORD 7454 ,7563 ,7671 ,7777 ,10104
.WORD 10211 ,10316 ,10422 ,10526 ,10631
.WORD 10734 ,11037 ,11141 ,11243 ,11344
.WORD 11445 ,11546 ,11646 ,11746 ,12045
.WORD 12144 ,12242 ,12340 ,12436 ,12533
.WORD 12630 ,12725 ,13021 ,13114 ,13210
.WORD 13302 ,13375 ,13467 ,13560 ,13652
.WORD 13742 ,14033 ,14123 ,14212 ,14301
.WORD 14370 ,14456 ,14544 ,14632 ,14717
.WORD 15004 ,15070 ,15154 ,15237 ,15323
.WORD 15405 ,15470 ,15552 ,15634 ,15715
.WORD 15776 ,16056 ,16136 ,16216 ,16276
.WORD 16355 ,16433 ,16512 ,16570 ,16645
.WORD 16723 ,16777 ,17054 ,17130 ,17204
.WORD 17260 ,17333 ,17406 ,17460 ,17532
.WORD 17604 ,17656 ,17727 ,20000