C23456789012345678901234567890123456789012345678901234567890123456789012
C LAST MODIFIED:
C MAY 16, 1991 TRAPS ADDED FOR ARGUMENTS OF ACOS AND ASIN
C FEBRUARY 21, 1991 (SUBROUTINE QROT AND ROTGEN ADDED)
C OCTOBER 25, 1989 TRANS2 CHANGED FROM CALCULATING XHAT=AHAT*EXP(2X) TO
C XHAT=AHAT*EXP(2X) TO XHAT=AHAT*EXP(X)
C TRANS8 AND QMULTC ADDED
C OCTOBER 27, 1989 TRANS9 ADDED
C
SUBROUTINE TRANS1(ULAT,ULONG,U)
C SUBROUTINE TO TRANSLATE LATITUDE AND LONGITUDE TO EUCLIDEAN
C COORDINATES
DOUBLE PRECISION ULAT,ULONG,PI180,U(3)
DATA PI180/.0174532925199432958/
U(3)=SIN(ULAT*PI180)
U(1)=COS(ULAT*PI180)*COS(ULONG*PI180)
U(2)=COS(ULAT*PI180)*SIN(ULONG*PI180)
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS6(U,ULAT,ULONG)
C SUBROUTINE TO TRANSLATE EUCLIDEAN COORDINATES INTO LATITUDE AND
C LONGITUDE
DOUBLE PRECISION ULAT,ULONG,PI180,U(3),TEMP
DATA PI180/.0174532925199432958/
TEMP=U(3)
IF (TEMP.GT.1.) TEMP=1.
IF (TEMP.LT.(-1.)) TEMP=-1.
ULAT=ASIN(TEMP)/PI180
IF ((U(1)**2+U(2)**2).LE.0.) THEN
ULONG=0.
RETURN
ENDIF
ULONG=ATAN2(U(2),U(1))/PI180
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS3(ALAT,ALONG,RHO,AHAT)
C SUBROUTINE TO TRANSLATE AXIS LATITUDE AND LONGITUDE, ANGLE OF ROTATION
C TO A ROTATION EXPRESSED AS A QUARTERNION
DOUBLE PRECISION ALAT,ALONG,RHO,PI180,AHAT(4),FACT
DATA PI180/.0174532925199432958/
AHAT(1)=COS(RHO*PI180/2.)
FACT=SIN(RHO*PI180/2.)
AHAT(4)=FACT*SIN(ALAT*PI180)
FACT=FACT*COS(ALAT*PI180)
AHAT(2)=FACT*COS(ALONG*PI180)
AHAT(3)=FACT*SIN(ALONG*PI180)
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS5(AHAT,ALAT,ALONG,RHO)
C SUBROUTINE TO TRANSLATE A QUARTERNION TO A AXIS LATITUDE, LONGITUDE
C AND ANGLE
DOUBLE PRECISION ALAT,ALONG,RHO,PI180,FACT,AHAT(4),TEMP
DATA PI180/.0174532925199432958/
TEMP=AHAT(1)
IF (TEMP.GT.1.) TEMP=1.
IF (TEMP.LT.(-1.)) TEMP=-1.
FACT=ACOS(TEMP)
RHO=FACT*2./PI180
TEMP=AHAT(4)/SIN(FACT)
IF (TEMP.GT.1.) TEMP=1.
IF (TEMP.LT.(-1.)) TEMP=-1.
ALAT=ASIN(TEMP)/PI180
ALONG=ATAN2(AHAT(3),AHAT(2))/PI180
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS2(X,AHAT,XHAT)
C SUBROUTINE TO EXPRESS XHAT=AHAT*EXP(X)--XHAT AND AHAT EXPRESSED AS
C QUARTERNIONS.
DOUBLE PRECISION THETA,FACT,X(3),AHAT(4),XHAT(4),EXPX(4)
THETA=SQRT(X(1)**2+X(2)**2+X(3)**2)
IF (THETA.LE.0.) THEN
DO 5 I=1,4
5 XHAT(I)=AHAT(I)
RETURN
ENDIF
EXPX(1)=COS(THETA/2.)
FACT=SIN(THETA/2.)/THETA
DO 10 I=2,4
10 EXPX(I)=FACT*X(I-1)
CALL QMULT(AHAT,EXPX,XHAT)
RETURN
END
C
C***********************************************************************
C
SUBROUTINE QMULTC(B,A,ABARB)
C SUBROUTINE TO CALCULATE ABAR*B FOR QUATERNIONS A AND B.
DOUBLE PRECISION B(1),A(1),ABAR(4),ABARB(4)
ABAR(1)=A(1)
DO 5 I=2,4
5 ABAR(I)=-A(I)
CALL QMULT(ABAR,B,ABARB)
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS8(A,X)
C SUBROUTINE TO SOLVE A=EXP(X) FOR X
DOUBLE PRECISION A(1),X(1),FACT,TEMP
IF (ABS(A(1)).GE.1.) THEN
DO 10 I=1,3
10 X(I)=0.
RETURN
ELSE IF (A(1).LT.0.) THEN
TEMP=-A(1)
IND=-1
ELSE IF (A(1).GE.0.) THEN
TEMP=A(1)
IND=1
ENDIF
FACT=ACOS(TEMP)
IF (ABS(FACT).LT.(1.D-30)) THEN
FACT=0.
ELSE
FACT=2*IND*FACT/SIN(FACT)
ENDIF
DO 5 I=1,3
5 X(I)=FACT*A(I+1)
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS4(AHAT,AMHAT)
C SUBROUTINE TO TRANSLATE A QUARTERNION INTO A ROTATION MATRIX
DOUBLE PRECISION A0,A1,A2,A3,AHAT(4),AMHAT(3,3)
A0=AHAT(1)
A1=AHAT(2)
A2=AHAT(3)
A3=AHAT(4)
AMHAT(1,1)=A0*A0+A1*A1-A2*A2-A3*A3
AMHAT(2,1)=2.*(A0*A3+A1*A2)
AMHAT(3,1)=2.*(A1*A3-A0*A2)
AMHAT(1,2)=2.*(A1*A2-A0*A3)
AMHAT(2,2)=A0*A0-A1*A1+A2*A2-A3*A3
AMHAT(3,2)=2.*(A0*A1+A2*A3)
AMHAT(1,3)=2.*(A0*A2+A1*A3)
AMHAT(2,3)=2.*(A2*A3-A0*A1)
AMHAT(3,3)=A0*A0-A1*A1-A2*A2+A3*A3
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS7(ALAT,ALONG,RHO,AMHAT)
C SUBROUTINE TO TRANSLATE AXIS LATITUDE AND LONGITUDE, ANGLE OF ROTATION
C INTO A ROTATION EXPRESSED AS A MATRIX
DOUBLE PRECISION ALAT,ALONG,RHO,AHAT(4),AMHAT(3,3)
CALL TRANS3(ALAT,ALONG,RHO,AHAT)
CALL TRANS4(AHAT,AMHAT)
RETURN
END
C
C***********************************************************************
C
SUBROUTINE QMULT(A,B,C)
DOUBLE PRECISION A(4),B(4),C(4)
C(1)=A(1)*B(1)-A(2)*B(2)-A(3)*B(3)-A(4)*B(4)
C(2)=A(1)*B(2)+A(2)*B(1)+A(3)*B(4)-A(4)*B(3)
C(3)=A(1)*B(3)+A(3)*B(1)+A(4)*B(2)-A(2)*B(4)
C(4)=A(1)*B(4)+A(4)*B(1)+A(2)*B(3)-A(3)*B(2)
RETURN
END
C
C***********************************************************************
C
INTEGER FUNCTION ICONV(K,L)
IF (L.LT.K) THEN
ICONV=(K*(K-1))/2+L
ELSE
ICONV=(L*(L-1))/2+K
ENDIF
RETURN
END
C
C***********************************************************************
C
SUBROUTINE TRANS9(A,H,RHO,ERRCHK)
C SUBROUTINE TO CONVERT A MATRIX A INTO AN AXIS H AND AN ANGLE RHO.
C RHO IS EXPRESSED IN RADIANS.
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
DIMENSION A(3,3),H(3)
RHO=(A(1,1)+A(2,2)+A(3,3)-1.)/2.
IF (ABS(RHO).GT.1.D0) THEN
RHO=0.
DO 1 I=1,3
1 H(I)=0.
ERRCHK=1.D20
RETURN
ENDIF
IF (RHO.GT.1.) RHO=1.
IF (RHO.LT.(-1.)) RHO=-1.
RHO=ACOS(RHO)
IF (RHO.EQ.0.) THEN
DO 2 I=1,3
2 H(I)=0.
ERRCHK=0.
RETURN
ENDIF
FACT=2.*SIN(RHO)
H(1)=(A(3,2)-A(2,3))/FACT
H(2)=(A(1,3)-A(3,1))/FACT
H(3)=(A(2,1)-A(1,2))/FACT
FACT=H(1)**2+H(2)**2+H(3)**2
ERRCHK=FACT-1.
FACT=SQRT(FACT)
DO 3 I=1,3
3 H(I)=H(I)/FACT
RETURN
END
C*********************************************************
SUBROUTINE QROT(A,U,V)
C SUBROUTINE TO COMPUTE V=AU WHERE A IS A ROTATION
C EXPRESSED AS A QUATERNION
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
DIMENSION A(4),U(3),V(3),AMHAT(3,3)
CALL TRANS4(A,AMHAT)
DO 10 I=1,3
V(I)=0.
DO 10 J=1,3
10 V(I) = V(I) + AMHAT(I,J)*U(J)
RETURN
END
C*****************************************************
SUBROUTINE ROTGEN(U,A)
C SUBROUTINE TO FIND A ROTATION A (EXPRESSED AS A QUATERNION)
C WHICH ROTATES THE NORTH POLE (0,0,1) TO THE VECTOR U
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
DIMENSION U(3), A(4)
TEMP=U(3)
IF (TEMP.GT.1.) TEMP=1.
IF (TEMP.LT.(-1.)) TEMP=-1.
THETA=ACOS(TEMP)
PHI=ATAN2(U(2),U(1))
A(1)=COS(THETA/2.)
A(2)=-SIN(THETA/2.)*SIN(PHI)
A(3)= SIN(THETA/2.)*COS(PHI)
A(4)=0.
RETURN
END