.TITLE ATRIG ;BY JOSE DIAZ .GLOBL .ASIN,.CALL,.SQRT,.ATAN2,.ACOS,.DEGRE,.SIN,.COS .MCALL .TLQ,.MOVF A0=R0 A1=R1 A2=R2 A3=R3 ; .MACRO .ASIN ANGLE,Z ;INVERSE SIN; BY JOSE DIAZ ;-PI/2 < ANGLE < PI/2. ANGLE = .ATAN2(Z/ SQRT(1-Z**2)) ASINA: .BLKW 2 ASINZ: .BLKW 2 .ASIN: STF A0,-(SP) LDF ASINZ,A0 MULF A0,A0 NEGF A0 ADDF #1,A0 CFCC BLT SINERR STF A0,.SQRT-4 CALL .SQRT .MOVF .SQRT-10,.ATAN2-10 .MOVF ASINZ,.ATAN2-4 CALL .ATAN2 .MOVF .ATAN2-14,ASINA LDF (SP)+,A0 RETURN SINERR: .TLQ <.ASIN OUT OF +-1 RANGE> JMP .CALL ;.MACRO .ACOS Z,ANGLE ;INVERSE COSINE ;0 <= ANGLE <= PI ACOSA: .BLKW 2 ;ANGLE = ATAN2(SQRT(1-Z**2)/Z) ACOSZ: .BLKW 2 .ACOS: STF A0,-(SP) LDF ACOSZ,A0 MULF A0,A0 NEGF A0 ADDF #1,A0 CFCC BLT COSERR STF A0,.SQRT-4 CALL .SQRT .MOVF .SQRT-10,.ATAN2-4 .MOVF ACOSZ,.ATAN2-10 CALL .ATAN2 .MOVF .ATAN2-14,ACOSA LDF (SP)+,A0 RETURN COSERR: .TLQ <.ACOS: OUTSIDE OF +-1> JMP .CALL ;INVERSE TANGENT FOR 2 ARGUMENTS; ;ANGLE=ATAN2(Y/X)-PI <= ANGLE <= PI ATAN2A: .BLKW 2 X: .BLKW 2 Y: .BLKW 2 .ATAN2: STF A0,-(SP) STF A1,-(SP) STF A2,-(SP) STF A3,-(SP) MOV R4,-(SP) TSTF X CFCC BEQ DENZER ;DENOMINATOR ZERO? LDF Y,A0 DIVF X,A0 CALL ATAN TSTF X CFCC BGT RET TSTF Y CFCC BLT QUAD3 ADDF PI,A0 BR RET QUAD3: SUBF PI,A0 BR RET DENZER: LDF PIDIV2,A0 ;DEMONINATOR=0 TSTF Y CFCC BGT RET ;IF A0>0, ATAN2=PI/2 BLT NUML .TLQ <.ATAN2 UNDEFINED FOR 0/0> JMP .CALL NUML: NEGF A0 ;IF A0<0, ATAN2=-PI/2 RET: TST .DEGRE ;CONVERT TO DEGREES? BEQ WB MULF PI180,A0 WB: STF A0,ATAN2A MOV (SP)+,R4 LDF (SP)+,A3 LDF (SP)+,A2 LDF (SP)+,A1 LDF (SP)+,A0 RETURN ;SUBROUTINE ATAN(A0) RETURNS ARCTANGENT OF A0 IN RADIANS. ;RESULT STORED IN A0. NEWTON-RAPHSON METHOD. ; -PI/2 <= ATAN(A0) <= PI/2 ATAN: MOV .DEGRE,-(SP) ;SAVE DEGREE-RADIAN FLAG CLR .DEGRE ;CALCULATE IN RADIANS CLR ATANF1 ;RESET FLAG FOR X>1 CLR ATANF2 ;RESET FLAG FOR X<0 BIG1: CMPF #1,A0 ;IS X>1? CFCC BLT GR1 TSTF A0 ;IS X<0? CFCC BLT SMN0 DOATN: MOV #15.,R4 ;NUMBER OF ITERATIONS FOR NEWTON-RAPHSON STF A0,XATAN ;STORE X IN XATAN TO FREE A0 FOR IN-LOOP USE LDF A0,A3 ;FIRST GUESS = X FOR ARCTANGENT ;TO SOLVE: TAN(Y) - X = 0, THAT IS, FIND Y, THE ARC TANGENT OF X. ;NEWTON'S METHOD: YNEW = Y + COS(Y)*(X*COS(Y) - SIN(Y)) LOOPAT: STF A3,.SIN-4 CALL .SIN LDF .SIN-10,A1 ;=SIN(Y) STF A3,.COS-4 CALL .COS LDF .COS-10,A0 ;=COS(Y) LDF A0,A2 MULF XATAN,A2 ;A2 = X*COS(Y) SUBF A1,A2 ;A2 = X*COS(Y) - SIN(Y) MULF A0,A2 ;A2 = COS(Y)*(X*COS(Y) - SIN(Y)) ADDF A2,A3 ;A3 = NEWGUESS = OLDGUESS + A2 SOB R4,LOOPAT ;ITERATE DONEAT: TST ATANF1 ;SPECIAL CASE: X>1? BGT POSATN NEXTAT: TST ATANF2 ;SPECIAL CASE: X<0? BGT NEGATN DNATAN: LDF A3,A0 MOV (SP)+,.DEGRE RETURN GR1: INC ATANF1 ;IF X>1, RAISE CORRESPONDING FLAG LDF #1,A2 ;FIND RECIPROCAL DIVF A0,A2 LDF A2,A0 BR DOATN ;DO ARCTANGENT SMN0: INC ATANF2 ;IF X<0, RAISE CORRESPONDING FLAG NEGF A0 ;MAKE X POSITIVE BR BIG1 ;SEE IF X > 1 POSATN: NEGF A3 ADDF PIDIV2,A3 ;IF X>1, ARCTAN(X)=PI/2-ATAN(1/X) BR NEXTAT ;NOW CHECK OTHER FLAG NEGATN: NEGF A3 ;IF X<0, ARCTAN(X)=-(ARCTAN(-X)) BR DNATAN PI: .FLT2 3.14159265 PIDIV2: .FLT2 1.5707963 ;pi/2 PI180: .FLT2 57.29578 ;180/PI XATAN: .BLKW 2 ATANF1: .BLKW 1 ATANF2: .BLKW 1 .END