subroutine aptrotq (rotm, ku, tol, theta, ux, uy, uz,
& thx, thy, thz, nerr)
ccbeg.
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c SUBROUTINE APTROTQ
c
c call aptrotq (rotm, ku, tol, theta, ux, uy, uz,
c & thx, thy, thz, nerr)
c
c Version: aptrotq Updated 1990 January 18 14:20.
c aptrotq Originated 1989 November 2 14:10.
c
c Author: Arthur L. Edwards, LLNL, L-298, Telephone (925) 422-4123.
c
c
c Purpose: To analyse the rotation operator rotm, to find the single
c rotation angle theta and the axis of rotation u = (ux, uy, uz),
c and the sequential rotation angles thx, thy, thz, around the
c x, y, and z axes, respectively.
c The operator rotm must be a unitary 3 x 3 matrix.
c The 3 row vectors must form a positive unit vector triple.
c The 3 column vectors must form a positive unit vector triple.
c The trace (sum of the diagonal elements) must be equal to
c trace = 1.0 + 2.0 * cos (theta).
c Angles may be in degrees (ku = 0) or radians (ku = 1).
c Sines and cosines less than tol will be treated as zero.
c Flag nerr indicates any errors in rotm or ku.
c
c Input: rotm, ku, tol.
c
c Output: theta, ux, uy, uz, thx, thy, thz, nerr.
c
c Glossary:
c
c ku Input Indicates angle units are degrees (0) or radians (1).
c
c nerr Output Indicates an input error, if not 0.
c 1 if matrix trace is bad.
c 2 if ku is not 0 or 1.
c
c rotm Output Rotation operator (a unitary 3 x 3 matrix).
c Must be sized rotm(3,3).
c
c theta Output Angle of rotation around axis (ux, uy, uz),
c counterclockwise when rotation axis is pointed at
c observer. Units are degrees (ku = 0) or radians
c (ku = 1).
c
c tol Input Numerical tolerance limit. Used to test and adjust
c unit vector components and point coordinates.
c On Cray computers, recommend 1.e-5 to 1.e-11.
c
c thx Output Angle of rotation around x axis. See theta.
c
c thy Output Angle of rotation around y ayis. See theta.
c
c thz Output Angle of rotation around z azis. See theta.
c
c ux,uy,uz Output The x, y, z components of the unit vector parallel
c to the rotation axis.
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
ccend.
c.... Dimensioned arguments.
c---- Rotation matrix.
dimension rotm (3,3)
c.... Local variables.
c---- Single axis rotation angle.
common /laptrotq/ angle
c---- Is x axis rotation angle.
common /laptrotq/ angx
c---- Is y axis rotation angle.
common /laptrotq/ angy
c---- Is z axis rotation angle.
common /laptrotq/ angz
c---- Cosine of rotation angle.
common /laptrotq/ costh
c---- Cosine of x axis rotation angle.
common /laptrotq/ cthx
c---- Cosine of y axis rotation angle.
common /laptrotq/ cthy
c---- Cosine of z axis rotation angle.
common /laptrotq/ cthz
c---- A very small number.
common /laptrotq/ fuz
c---- Mathematical constant, pi.
common /laptrotq/ pi
c---- Sine of rotation angle.
common /laptrotq/ sinth
c---- Sine of x axis rotation angle.
common /laptrotq/ sthx
c---- Sine of y axis rotation angle.
common /laptrotq/ sthy
c---- Sine of z axis rotation angle.
common /laptrotq/ sthz
c---- Sum of squares of sine, cosine.
common /laptrotq/ sumsq
cbugc***DEBUG begins.
cbugc---- Array index.
cbug common /laptrotq/ i
cbugc---- Array index.
cbug common /laptrotq/ j
cbugc---- Array index.
cbug common /laptrotq/ n
cbugc---- Square of magnitude of row vector.
cbug common /laptrotq/ sumrow (3)
cbugc---- Square of magnitude of column vector.
cbug common /laptrotq/ sumcol (3)
cbugc---- 1.0 + 2.0 * costh.
cbug common /laptrotq/ trace
cbug 9901 format (/ 'aptrotq rotm=',3(/ 2x,1p3e22.14))
cbug 9902 format (' sumrow=',1p3e22.14 /
cbug & ' sumcol=',1p3e22.14)
cbug 9903 format (' trace= ',1pe22.14)
cbug write ( 3, 9901) ((rotm(i,j), j = 1, 3), i = 1, 3)
cbug do 981 n = 1, 3
cbug sumrow(n) = rotm(n,1)**2 + rotm(n,2)**2 + rotm(n,3)**2
cbug sumcol(n) = rotm(1,n)**2 + rotm(2,n)**2 + rotm(3,n)**2
cbug 981 continue
cbug write ( 3, 9902) (sumrow(n), n = 1, 3), (sumcol(n), n = 1, 3)
cbug trace = rotm(1,1) + rotm(2,2) + rotm(3,3)
cbug write ( 3, 9903) trace
cbugc***DEBUG ends.
c.... Initialize.
c---- A very small number.
fuz = 1.e-99
c---- Mathematical constant, pi.
pi = 3.14159265358979323
nerr = 0
theta = -1.e+99
ux = -1.e+99
uy = -1.e+99
uz = -1.e+99
thx = -1.e+99
thy = -1.e+99
thz = -1.e+99
c.... Find the single-axis angle of rotation, and the axis of rotation.
costh = 0.5 * (rotm(1,1) + rotm(2,2) + rotm(3,3) - 1.0)
if (abs (1.0 - costh) .le. tol) then
costh = 1.0
sinth = 0.0
elseif (abs (1.0 + costh) .le. tol) then
costh = -1.0
sinth = 0.0
elseif (abs (costh) .le. tol) then
costh = 0.0
sinth = 1.0
c++++ Matrix trace is bad.
elseif (abs (costh) .gt. 1.0) then
nerr = 1
cbugc***DEBUG begins.
cbug 9911 format ('aptrotq error. costh=',1pe22.14)
cbug write ( 3, 9911) costh
cbugc***DEBUG ends.
go to 210
else
sinth = sqrt ((1.0 - costh) * (1.0 + costh))
endif
angle = acos (costh)
c.... Get rotation angle in radians.
if (ku .eq. 0) then
theta = angle * 180.0 / pi
elseif (ku .eq. 1) then
theta = angle
else
nerr = 2
cbugc***DEBUG begins.
cbug 9921 format ('aptrotq error. ku=',i6)
cbug write ( 3, 9921) ku
cbugc***DEBUG ends.
go to 210
endif
if (sinth .gt. tol) then
ux = 0.5 * (rotm(3,2) - rotm(2,3)) / sinth
uy = 0.5 * (rotm(1,3) - rotm(3,1)) / sinth
uz = 0.5 * (rotm(2,1) - rotm(1,2)) / sinth
c---- If theta is 0 or 180 degrees.
else
ux = sqrt (0.5 * (rotm(1,1) + 1.0))
c++++ Sign may change.
uy = sqrt (0.5 * (rotm(2,2) + 1.0))
c++++ Sign may change.
uz = sqrt (0.5 * (rotm(3,3) + 1.0))
if ((ux .ge. uy) .and. (ux .ge. uz)) then
uy = 0.5 * rotm(1,2) / ux
uz = 0.5 * rotm(1,3) / ux
elseif ((uy .ge. uz) .and. (uy .ge. ux)) then
ux = 0.5 * rotm(1,2) / uy
uz = 0.5 * rotm(2,3) / uy
elseif ((uz .ge. ux) .and. (uz .ge. uy)) then
ux = 0.5 * rotm(1,3) / uz
uy = 0.5 * rotm(2,3) / uz
endif
endif
cbugc***DEBUG begins.
cbug 9931 format (5x,'theta=',f12.6,' costh=',f10.6,' sinth=',f10.6 /
cbug & ' ux,uy,uz=',1p3e22.14)
cbug write ( 3, 9931) theta, costh, sinth, ux, uy, uz
cbugc***DEBUG ends.
c.... Find the 3 angles for sequential rotation around the x, y, and z axes.
sthy = -rotm(3,1)
if (abs (1.0 - sthy) .le. tol) then
sthy = 1.0
cthy = 0.0
elseif (abs (1.0 + sthy) .le. tol) then
sthy = -1.0
cthy = 0.0
elseif (abs (sthy) .le. tol) then
sthy = 0.0
cthy = 1.0
c++++ Matrix trace is bad.
elseif (abs (sthy) .gt. 1.0) then
nerr = 1
cbugc***DEBUG begins.
cbug 9941 format ('aptrotq error. sthy=',1pe22.14)
cbug write ( 3, 9941) sthy
cbugc***DEBUG ends.
go to 210
else
cthy = sqrt ((1.0 - sthy) * (1.0 + sthy))
endif
if (cthy .gt. tol) then
sthx = rotm(3,2) / cthy
cthx = rotm(3,3) / cthy
sthz = rotm(2,1) / cthy
cthz = rotm(1,1) / cthy
c---- If y angle is 90 or -90 degrees.
else
cthx = 1.0
sthx = 0.0
cthz = rotm(2,2)
sthz = -rotm(1,2)
endif
sumsq = sthx**2 + cthx**2
if (abs (sumsq - 1.0) .gt. tol) then
nerr = 1
cbugc***DEBUG begins.
cbug 9951 format ('aptrotq error. sthx**2 + cthx**2=',1pe22.14)
cbug write ( 3, 9951) sumsq
cbugc***DEBUG ends.
go to 210
endif
sumsq = sthz**2 + cthz**2
if (abs (sumsq - 1.0) .gt. tol) then
nerr = 1
cbugc***DEBUG begins.
cbug 9961 format ('aptrotq error. sthz**2 + cthz**2=',1pe22.14)
cbug write ( 3, 9961) sumsq
cbugc***DEBUG ends.
go to 210
endif
c.... Get rotation angles in radians.
angx = atan2 (sthx, (cthx + fuz))
angy = atan2 (sthy, (cthy + fuz))
angz = atan2 (sthz, (cthz + fuz))
if (ku .eq. 0) then
thx = angx * 180.0 / pi
thy = angy * 180.0 / pi
thz = angz * 180.0 / pi
elseif (ku .eq. 1) then
thx = angx
thy = angy
thz = angz
else
nerr = 2
cbugc***DEBUG begins.
cbugc____ 9921 format ("aptrotq error. ku=",i6)
cbug write ( 3, 9921) ku
cbugc***DEBUG ends.
go to 210
endif
cbugc***DEBUG begins.
cbug 9971 format (5x,'th(x,y,z)=',3f12.6 /
cbug & ' cth,sth(x,y,z)=',6f10.6)
cbug write ( 3, 9971) thx, thy, thz,
cbug & cthx, sthx, cthy, sthy, cthz, sthz
cbugc***DEBUG ends.
210 return
c.... End of subroutine aptrotq. (+1 line.)
end
UCRL-WEB-209832