C PRP.FOR, Woltring 1988-02-22 C C*********************************************************************** C C PRP C C Purpose: C ******* C C Testprogramme PRP for subroutines HCPR and RPHC, for conversion C between helical or cardanic attitude angles and attitude matri- C ces. The values PHI1, PHI2, and PHI3 denote the input angles, C and IJK denotes the chosen attitude convention. If IJK.eq.0, C the helical convention is chosen, if IJK.eq. +1, +2, or +3, a C cyclic cardan convention i,j,k, with i=IJK is adopted, C C Rijk = Ri(PHIi) * Rj(PHIj) * Rk(PHIk) C C and if IJK.eq. -1, -2, or -3, an anticyclic cardan convention C k,j,i, with i=-IJK is adopted, C C Rkji = Rk(PHIk) * Rj(PHIj) * Ri(PHIi) C C PRP types the 7 attitude conventions and returns to accept new C input. If all angles are zero, PRP terminates. C C Reference: C ********* C C H.J. Woltring (1989), One Hundred Years Photogrammetry in Bio- C locomotion. In: V. Tosi & A. Cappozzo (Eds), Proc. of the Sympo- C sium on Biolocomotion: a Century of Research Using Moving Pictu- C res (Formia, Italy, April 1989; in print). C C*********************************************************************** C PROGRAM PRP C PARAMETER (PI=3.1415926535898, DEGRAD=PI/180.0, RADDEG=180.0/PI) D REAL*8 DSUM, DBLE DIMENSION P(3), PP(3), R(3,3), Q(3,-3:3) C 10 TYPE 700 700 FORMAT('$ phi1, phi2, phi3; ijk = ') !in degrees ACCEPT 710, P, IJK 710 FORMAT(3E15.0,I5) IF ( (P(1).eq.0.0).and.(P(2).eq.0.0).and.(P(3).eq.0.0) ) 1 STOP 'PRP: ready' DO 20 I=1,3 PP(I) = DEGRAD * P(I) !convert degrees to radians 20 CONTINUE C CALL HCPR(IJK,PP,R) C D DSUM = -3D0 D DO 50 I=1,3 D DO 40 J=1,3 D DO 30 K=1,3 D DSUM = DSUM + DBLE(R(K,I))*R(K,J) D 30 CONTINUE D 40 CONTINUE D 50 CONTINUE D RNORM = ABS( SNGL(DSUM) ) / 9.0 D TYPE 720, RNORM, ((R(I,J),J=1,3),I=1,3) D 720 FORMAT('0||R''R|| =',1PE10.3/'0 R:',0P3F9.5,2(/4X,3F9.5)) C DO 60 I=-3,3 CALL RPHC(I,R,Q(1,I)) 60 CONTINUE C TYPE 730, (I,I=-3,3), ((RADDEG*Q(J,I),I=-3,3),J=1,3) 730 FORMAT('0 I :',I5,6I9/ 1 ' phi1:',7F9.3/' phi2:',7F9.3/' phi3:',7F9.3/) GO TO 10 C END C HCPR.FOR, Woltring 1988-02-22 C C*********************************************************************** C C Subroutine HCPR C C Function: C ******** C C Subroutine to convert a given helical or cardanic set of attitu- C de angles into an orthonormal attitude matrix. C C Calling convention: C ****************** C C CALL HCPR ( IJK, PHI, R ) C C Meaning of parameters: C ********************* C C IJK (I) Attitude convention switch: C 0 : Finite helical rotation angles C +1,+2,+3: Cyclic cardanic convention, i=+IJK C Rijk = Ri(PHIi) * Rj(PHIj) * Rk(PHIk) C -1,-2,-3: Anticyclic cardanic convention, i=-IJK C Rkji = Rk(PHIk) * Rj(PHIj) * Ri(PHIi) C PHI(3) (I) Input attitude angles C R(3,3) (O) Output attitude matrix C C*********************************************************************** C SUBROUTINE HCPR ( IJK, PHI, R ) C PARAMETER (SQRTOL=1E-4) !SQRT(relative machine tolerance) DIMENSION PHI(3), R(3,3) SQ(SI) = SI * SI C IF (IJK.eq.0) THEN C Finite Helical Rotations SK = SQRT( SQ(PHI(1)) + SQ(PHI(2)) + SQ(PHI(3)) ) !theta CI = COS(SK) !cos(theta) SI = SIN(SK) !sin(theta) IF (ABS(SK).le.SQRTOL) THEN CJ = 0.5 SJ = 1.0 ELSE CJ = 2.0 * SQ( SIN(0.5*SK) / SK ) !(1 - cos(theta))/theta**2 SJ = SI / SK ! sin(theta) /theta ENDIF R(3,2) = SJ * PHI(1) R(1,3) = SJ * PHI(2) R(2,1) = SJ * PHI(3) R(2,3) = -R(3,2) R(3,1) = -R(1,3) R(1,2) = -R(2,1) DO 20 I=1,3 CK = CJ * PHI(I) R(I,I) = CI DO 10 J=1,I SI = CK * PHI(J) R(I,J) = R(I,J) + SI IF (I.ne.J) R(J,I) = R(J,I) + SI 10 CONTINUE 20 CONTINUE C End of Finite Helical Rotations ELSE C Cardanic Rotations I = MOD(IABS(IJK)-1,3) + 1 J = MOD(I,3) + 1 K = MOD(J,3) + 1 IF (IJK.gt.0) THEN C Cyclic: ijk SI = SIN( PHI(I)) SJ = SIN( PHI(J)) SK = SIN( PHI(K)) ELSE C Anticyclic: kji SI = SIN(-PHI(I)) SJ = SIN(-PHI(J)) SK = SIN(-PHI(K)) ENDIF CI = COS( PHI(I)) CJ = COS( PHI(J)) CK = COS( PHI(K)) SISK = SI * SK CISK = CI * SK SICK = SI * CK CICK = CI * CK R(I,I) = CJ * CK R(J,J) = CICK - SJ*SISK R(K,K) = CI * CJ IF (IJK.gt.0) THEN C Cyclic: ijk R(J,I) = CISK + SJ*SICK R(K,I) = SISK - SJ*CICK R(I,J) = -CJ*SK R(K,J) = SICK + SJ*CISK R(I,K) = SJ R(J,K) = -SI*CJ ELSE C Anticyclic: kji R(I,J) = CISK + SJ*SICK R(I,K) = SISK - SJ*CICK R(J,I) = -CJ*SK R(J,K) = SICK + SJ*CISK R(K,I) = SJ R(K,J) = -SI*CJ ENDIF C End of Cardanic Rotations ENDIF C RETURN END C RPHC.FOR, Woltring 1988-02-19 C C*********************************************************************** C C Subroutine RPHC C C Function: C ******** C C Subroutine to convert a given orthonormal attitude matrix into a C helical or cardanic set of attitude angles. C C Calling convention: C ****************** C C CALL RPHC ( IJK, R, PHI ) C C Meaning of parameters: C ********************* C C IJK (I) Attitude convention switch: C 0 : Finite helical rotation angles C +1,+2,+3: Cyclic cardanic convention, i=+IJK C Rijk = Ri(PHIi) * Rj(PHIj) * Rk(PHIk) C -1,-2,-3: Anticyclic cardanic convention, i=-IJK C Rkji = Rk(PHIk) * Rj(PHIj) * Ri(PHIi) C R(3,3) (I) Input attitude matrix C PHI(3) (O) Output attitude angles C C*********************************************************************** C SUBROUTINE RPHC ( IJK, R, PHI ) C PARAMETER ( TOL=1E-8, SQRTOL=1E-4 ) !machine tolerances DIMENSION R(3,3), PHI(3) SQ(SI) = SI * SI C IF (IJK.EQ.0) THEN C Finite Helical Rotations PHI(1) = 0.5 * ( R(3,2) - R(2,3) ) PHI(2) = 0.5 * ( R(1,3) - R(3,1) ) PHI(3) = 0.5 * ( R(2,1) - R(1,2) ) SI = SQRT( SQ(PHI(1)) + SQ(PHI(2)) + SQ(PHI(3)) ) CI = AMAX1( -1.0, 0.5*(R(1,1)+R(2,2)+R(3,3)-1.0) ) SK = ATAN2(SI,CI) !theta IF (SI+CI.gt.0.0) THEN C 0 .le. theta .lt. 3*PI/4 IF (SI.gt.SQRTOL) THEN CK = SK / SI !theta / sin(theta) DO 10 I=1,3 PHI(I) = CK * PHI(I) 10 CONTINUE ENDIF ELSE C 3*PI/4 .le. theta .le. PI K = 0 CK = 0.0 DO 20 I=1,3 IF (ABS(PHI(I)).ge.ABS(CK)) THEN K = I CK = PHI(I) ENDIF 20 CONTINUE DO 30 I=1,3 IF (I.eq.K) THEN PHI(I) = R(I,K) - CI ELSE PHI(I) = 0.5 * ( R(I,K) + R(K,I) ) ENDIF 30 CONTINUE CK = SK/SIGN( SQRT(SQ(PHI(1))+SQ(PHI(2))+SQ(PHI(3)) ),CK) DO 40 I=1,3 PHI(I) = CK * PHI(I) 40 CONTINUE ENDIF C End of Finite Helical Rotations ELSE C Cardanic Rotations I = MOD(IABS(IJK)-1,3) + 1 J = MOD(I,3) + 1 K = MOD(J,3) + 1 IF (IJK.gt.0) THEN C Cyclic: ijk SIPK = R(K,J) + R(J,I) ! (1+sj)*sin(Pi+Pk) CIPK = R(J,J) - R(K,I) ! (1+sj)*cos(Pi+Pk) SIMK = R(K,J) - R(J,I) ! (1-sj)*sin(Pi-Pk) CIMK = R(J,J) + R(K,I) ! (1-sj)*cos(Pi-Pk) ELSE C Anticyclic: kji SIPK = R(J,K) + R(I,J) ! (1+sj)*sin(Pi+Pk) CIPK = R(J,J) - R(I,K) ! (1+sj)*cos(Pi+Pk) SIMK = R(J,K) - R(I,J) ! (1-sj)*sin(Pi-Pk) CIMK = R(J,J) + R(I,K) ! (1-sj)*cos(Pi-Pk) ENDIF IF (SQ(SIPK)+SQ(CIPK).gt.TOL) THEN PIPK = ATAN2(SIPK,CIPK) ELSE PIPK = 0.0 ENDIF IF (SQ(SIMK)+SQ(CIMK).gt.TOL) THEN PIMK = ATAN2(SIMK,CIMK) ELSE PIMK = 0.0 ENDIF IF (IJK.gt.0) THEN C Cyclic: ijk PHI(I) = 0.5 * ( PIPK + PIMK ) PHI(K) = 0.5 * ( PIPK - PIMK ) PHI(J) = ATAN2( 2.0*R(I,K), 1 COS(PHI(K))*R(I,I) - SIN(PHI(K))*R(I,J) - 2 SIN(PHI(I))*R(J,K) + COS(PHI(I))*R(K,K) ) ELSE C Anticyclic: kji PHI(I) = -0.5 * ( PIMK + PIPK ) PHI(K) = 0.5 * ( PIMK - PIPK ) PHI(J) = ATAN2(-2.0*R(K,I), 1 COS(PHI(K))*R(I,I) + SIN(PHI(K))*R(J,I) + 2 SIN(PHI(I))*R(K,J) + COS(PHI(I))*R(K,K) ) ENDIF C End of Cardanic Rotations ENDIF C RETURN END