perm filename ARITH.PAL[V,VDS] blob
sn#266460 filedate 1977-04-04 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00008 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 "CROSS" - COMPUTES THE CROSS PRODUCT OF TWO VECTORS
C00017 00008 TABLES OF SINE/COSINE AND ARC-TANGENT
C00021 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.
;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 #-6,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)]
TST R3 ;ROUND OFF
BPL .+4
INC R2
ADD R2,R0 ;NOW HAVE ABS(SIN(X))
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)]
TST R3 ;ROUND OFF
BPL .+4
INC R2
ADD R2,R1 ;NOW HAVE ABS(COS(X))
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 LOOK 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.
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 #-1,R0
DIV R2,R0
CLR R1
ASHC #-8.,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
MUL R1,R2
CLR R1
ASHC #-1,R0
ADD R3,R1
ADC R0
ADD R2,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 #2,R0 ;ADJUST BY 2↑2
SOB R4,SQRT3
SQRT4: MOV R0,R2
MOV R1,R3
ASHC #-2,R2
BIT #20000,R0
BNE HALF
MUL #64000,R0
ADD #4660,R0
BR ITER
HALF: MUL #45000,R0
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 "T2" IS MULTIPLIED BY "T3" AND THE RESULT IS STORED IN
;"T1". IT IS ASSUMED THAT THE TRANSFORMS ARE STORED AS 3 X 4
;COLUMN MATRICES. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #T1,R0 ;LOAD ADDRESS OF TRANSFORMS
; MOV #T2,R1
; MOV #T3,R2
; JSR PC,MATMUL ;T1 ← T2 X T3
;
;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 ARE NOT MODIFIED
MATMUL: MOV R2,-(SP) ;SAVE POINTERS
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
MOV (SP)+,R2
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 #2,R2 ;NORMALIZE AND ROUND
TST R3
BPL .+4
INC R2
RTS PC
;END OF "MUL3X3"
;"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 #2,R2
TST R3
BPL .+4
INC R2
RTS PC
;END OF "CROSS"
;TABLES OF SINE/COSINE AND ARC-TANGENT
SINE: .WORD 0 ,311 ,622 ,1133 ,1444
.WORD 1755 ,2265 ,2576 ,3106 ,3416
.WORD 3726 ,4235 ,4544 ,5053 ,5361
.WORD 5667 ,6174 ,6501 ,7006 ,7312
.WORD 7615 ,10120 ,10422 ,10723 ,11224
.WORD 11524 ,12023 ,12322 ,12620 ,13114
.WORD 13411 ,13704 ,14176 ,14467 ,14757
.WORD 15247 ,15535 ,16022 ,16306 ,16571
.WORD 17053 ,17334 ,17614 ,20072 ,20347
.WORD 20623 ,21075 ,21347 ,21616 ,22065
.WORD 22332 ,22576 ,23040 ,23301 ,23540
.WORD 23776 ,24232 ,24465 ,24716 ,25145
.WORD 25373 ,25617 ,26041 ,26262 ,26501
.WORD 26717 ,27132 ,27344 ,27554 ,27762
.WORD 30166 ,30371 ,30571 ,30770 ,31164
.WORD 31357 ,31550 ,31737 ,32123 ,32306
.WORD 32467 ,32645 ,33022 ,33175 ,33345
.WORD 33513 ,33660 ,34022 ,34161 ,34317
.WORD 34453 ,34604 ,34733 ,35060 ,35202
.WORD 35323 ,35441 ,35555 ,35666 ,35775
.WORD 36102 ,36205 ,36305 ,36403 ,36477
.WORD 36570 ,36657 ,36743 ,37025 ,37105
.WORD 37162 ,37235 ,37305 ,37353 ,37417
.WORD 37460 ,37517 ,37553 ,37605 ,37634
.WORD 37661 ,37704 ,37724 ,37741 ,37754
.WORD 37765 ,37773 ,37777
COSINE: .WORD 40000
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