module rotlib_m !! !! Euler angle ranges: !! !! * XYZ, ZYX: phi in [-pi, pi], theta in [-pi/2, pi/2], psi in [-pi, pi] !! * XZY, YZX: phi in [-pi, pi], theta in [-pi, pi], psi in [-pi/2, pi/2] !! * ZXY, YXZ: phi in [-pi/2, pi/2], theta in [-pi, pi], psi in [-pi, pi] !! !! Euler angle sequence: XYZ (world). First rotation about X, second rotation !! about Y, and the third rotation about Z axis of the world(i.e. fixed) frame. !! This is the same as the sequence used in *Blender*. !! !! In contrast, the XYZ sequence is understood in the Aerospace community as: !! XYZ: First rotation about Z-axis, second rotation about Y-axis, and the third !! rotation about X-axis of the body frame. !! !! *Reference*: <http://www.geometrictools.com/Documentation/EulerAngles.pdf> use constants_m, only: math_pi, math_pi_2 use utils_math_m implicit none public private :: factor_rotmat, shift_tensor2, shift_tensor3, & rotmat_xyz, rotmat_xzy, rotmat_yxz, & rotmat_yzx, rotmat_zxy, rotmat_zyx, & factor_rotmat_xyz, factor_rotmat_xzy, factor_rotmat_yxz, & factor_rotmat_yzx, factor_rotmat_zxy, factor_rotmat_zyx contains !*****************************************************************************! ! UTILITY FUNCTIONS ! !*****************************************************************************! subroutine align(v, old, new, valigned) !! Rotates vectors such that a set of mutually orthogonal unit vectors !! aligns with another set of mutually orthogonal unit vectors. !! !! The vectors `v` are rotated using a rotation matrix that, when applied !! to the unit vectors `old`, aligns them in the direction of the unit !! vectors `new`. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to align. real(rp), dimension(:,:), intent(in) :: old !! *(3,m)*. A set of at most three mutually orthogonal unit vectors. !! `m` must be one of 1, 2 or 3. real(rp), dimension(:,:), intent(in) :: new !! *(3,m)*. Another set of mutually orthogonal unit vectors of the same !! shape as `old`. The angles between the vectors must be the same as !! those of `old`. real(rp), dimension(:,:), intent(out) :: valigned !! *(3,n)*. Aligned vectors, of the same shape as `v`. real(rp) :: axis(3), z_old(3), z_new(3), axes_old(3,3), & axes_new(3,3), dcm(3,3) real(rp) :: cos_angle, angle integer :: n if ( any(shape(old) /= shape(new)) ) then write(*,*) 'Shape mismatch between `old` and `new`.' write(*,*) 'Shape of `old` = ', shape(old) write(*,*) 'Shape of `new` = ', shape(new) stop 'Shape mismatch.' end if n = size(old, dim=2) if (n == 1) then !Angle between old and new cos_angle = dot_product(old(:,1), new(:,1)) if ( (cos_angle > 1.0_rp) .or. (cos_angle < -1.0_rp) ) then if ( isclose(abs(cos_angle), 1.0_rp, 1e-9_rp, 1e-15_rp) ) then cos_angle = sign(1.0_rp, cos_angle) else stop 'Cos of angle out-of-range.' end if end if if ( isclose(cos_angle, 1.0_rp, 1e-9_rp, 1e-15_rp) ) then !Angle = zero: No rotation needed valigned = v else if ( isclose(cos_angle, -1.0_rp, 1e-9_rp, 1e-15_rp) ) then !Angle = pi: Rotation axis not unique. We flip the ! vectors `v` to align. valigned = -v else angle = acos(cos_angle) !Axis of rotation call cross(old, new, axis) axis = axis/norm2(axis) call aa_rotate_vectors(v, axis, angle, valigned) end if else if (n == 2) then !The third vector orthogonal to `old` call cross( old(:,1), old(:,2), z_old ) z_old = z_old/norm2(z_old) !The third vector orthogonal to `new` call cross( new(:,1), new(:,2), z_new ) z_new = z_new/norm2(z_new) !The `old` and new `frames` axes_old(:,1:2) = old; axes_old(:,3) = z_old axes_new(:,1:2) = new; axes_new(:,3) = z_new call dcm_from_axes(axes_old, axes_new, dcm) call dcm_rotate_vectors(v, dcm, valigned) else if (n == 3) then call dcm_from_axes(old, new, dcm) call dcm_rotate_vectors(v, dcm, valigned) end if end subroutine !************************************************************************ logical function mat_is_rotmat(mat) !! Returns `.true.` if a (3,3) matrix is a rotation matrix. real(rp), dimension(3,3), intent(in) :: mat !! (3,3). Matrix to check. real(rp), dimension(3,3) :: mat_trans real(rp), dimension(3,3) :: mat_prod logical :: det_is_one logical :: is_orthogonal real(rp), dimension(3,3) :: eye call identity(eye) mat_prod = matmul(mat, transpose(mat)) det_is_one = isclose(det(mat), 1.0_rp) is_orthogonal = allclose(mat_prod, eye) mat_is_rotmat = (is_orthogonal .and. det_is_one) end function !******************************************************************************! ! QUATERNION ! !******************************************************************************! subroutine quat_rand(q) !! Returns a random unit quaternion. real(rp), dimension(4), intent(out) :: q !! Unit quaternion. real(rp), dimension(3) :: axis real(rp) :: angle call aa_rand(axis, angle) call aa_to_quat(axis, angle, q) end subroutine !************************************************************************ subroutine quat_identity(q) !! Returns the identity unit quaternion. real(rp), dimension(4), intent(out) :: q !! Unit quaternion. q(1) = 1.0_rp; q(2) = 0.0_rp; q(3) = 0.0_rp; q(4) = 0.0_rp end subroutine !************************************************************************ subroutine quat_conjugated(q) !! Conjugates a quaternion in-place. real(rp), dimension(4), intent(in out) :: q !! On entry, a quaternion. On return, the conjugated quaternion. q(2:4) = -q(2:4) end subroutine !************************************************************************ subroutine quat_inverted(q) !! Inverts a quaternion in-place. real(rp), dimension(4), intent(in out) :: q !! On entry, a quaternion. On return, the inverted quaternion. call quat_conjugated(q) call quat_normalized(q) end subroutine !************************************************************************ subroutine quat_normalized(q) !! Normalizes a quaternion in-place. real(rp), dimension(4), intent(in out) :: q !! On entry, a quaternion. On return, the normalized quaternion. q = q /norm2(q) end subroutine !************************************************************************ logical function quat_is_normalized(q) !! Returns `.true.` if quaternion is normalized, i.e. if it is a unit !! quaternion real(rp), dimension(4), intent(in) :: q !! A quaternion real(rp) :: norm norm = norm2(q) if (isclose(norm, 1.0_rp, rel_tol=1e-14_rp)) then quat_is_normalized = .true. else quat_is_normalized = .false. end if end function !************************************************************************ subroutine quat_prod (p, q, pq, normalize) !! Calculates the product of two quaternions real(rp), dimension(4), intent(in) :: p !! Quaternion. real(rp), dimension(4), intent(in) :: q !! Quaternion. real(rp), dimension(4), intent(out) :: pq !! Product of `p` and `q`. This is a unit quaternion if `normalize` !! is `.true.`. logical, intent(in), optional :: normalize !! Whether to normalize the product, defaults to `.true.`. logical :: normalize_ = .true. real(rp), dimension(4,4) :: prod_mat if (present(normalize)) normalize_ = normalize pq(1) = p(1)*q(1) - p(2)*q(2) - p(3)*q(3) - p(4)*q(4) pq(2) = p(2)*q(1) + p(1)*q(2) - p(4)*q(3) + p(3)*q(4) pq(3) = p(3)*q(1) + p(4)*q(2) + p(1)*q(3) - p(2)*q(4) pq(4) = p(4)*q(1) - p(3)*q(2) + p(2)*q(3) + p(1)*q(4) if (normalize_) call quat_normalized(pq) end subroutine !************************************************************************ function quat_angle_between(p, q) result(angle) !! Returns the angle between two _unit_ quaternions real(rp), dimension(4), intent(in) :: p !! Unit quaternion. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp) :: angle, cos_angle !! Angle in radian. cos_angle = dot_product(p, q) if ( (cos_angle > 1.0_rp) .or. (cos_angle < -1.0_rp) ) then if ( isclose(abs(cos_angle), 1.0_rp, 1e-9_rp, 1e-15_rp) ) then cos_angle = sign(1.0_rp, cos_angle) else stop 'Cos of angle out-of-range.' end if end if angle = acos(cos_angle) end function !************************************************************************ subroutine quat_interpolate(q1, q2, t, q) !! Interpolates between two _unit_ quaternions. real(rp), dimension(4), intent(in) :: q1 !! Unit quaternion. real(rp), dimension(4), intent(in) :: q2 !! Unit quaternion. real(rp), intent(in) :: t !! Interpolation factor, *0 <= t <= 1*. real(rp), dimension(4), intent(out) :: q !! Interpolated unit quaternion. real(rp) :: theta theta = quat_angle_between(q1, q2) q = (q1*sin((1.0_rp-t)*theta) + q2*sin(t*theta))/sin(theta) end subroutine !************************************************************************ subroutine quat_deriv_to_angvel_mat(qdot, mat) !! Returns the matrix mapping the derivative of a unit quaternion to !! angular velocity. real(rp), dimension(4), intent(in) :: qdot !! Derivative of a unit quaternion. real(rp), dimension(3,4), intent(out) :: mat !! Angular velocity matrix. mat = reshape([-qdot(2), -qdot(3), -qdot(4), & qdot(1), qdot(4), -qdot(3), & -qdot(4), qdot(1), qdot(2), & qdot(3), -qdot(2), qdot(1) ], [3,4]) mat = 2*mat end subroutine !************************************************************************ subroutine quat_deriv_to_angvel (q, qdot, angvel) !! Calculates the angular velocity from a unit quaternion and its time !! derivative. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(4), intent(in) :: qdot !! Derivative of `q`. real(rp), dimension(3), intent(out) :: angvel !! Angular velocity. real(rp) :: mat(3,4) call quat_deriv_to_angvel_mat(q, mat) angvel = matmul(mat, qdot) end subroutine !************************************************************************ subroutine quat_deriv_from_angvel_mat (q, mat) !! Returns the matrix mapping angular velocity to time derivative of a unit !! quaternion. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(4,3), intent(out) :: mat !! Quaternion derivative matrix. mat = reshape([-q(2), q(1), -q(4), q(3), & -q(3), q(4), q(1), -q(2), & -q(4), -q(3), q(2), q(1)], [4,3]) mat = mat/2 end subroutine !************************************************************************ subroutine quat_deriv_from_angvel(q, angvel, qdot) !! Calculates the time derivative of a unit quaternion from angular velocity. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(3), intent(in) :: angvel !! Angular velocity. real(rp), dimension(4), intent(out) :: qdot !! Time derivative of `q`. real(rp) :: mat(4,3) call quat_deriv_from_angvel_mat(q, mat) qdot = matmul(mat, angvel) end subroutine !************************************************************************ subroutine quat_rotmat (q, rotmat) !! Returns the rotation matrix corresponding to a unit quaternion. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(3,3), intent(out) :: rotmat !! Rotation matrix. real(rp) :: q1sq, q2sq, q3sq, q4sq real(rp) :: q1q2, q1q3, q1q4, q2q3, q2q4, q3q4 q1sq = q(1)*q(1) q2sq = q(2)*q(2) q3sq = q(3)*q(3) q4sq = q(4)*q(4) q1q2 = q(1)*q(2) q1q3 = q(1)*q(3) q1q4 = q(1)*q(4) q2q3 = q(2)*q(3) q2q4 = q(2)*q(4) q3q4 = q(3)*q(4) rotmat(1,1) = 2*(q1sq + q2sq) - 1.0_rp rotmat(2,1) = 2*(q2q3 + q1q4) rotmat(3,1) = 2*(q2q4 - q1q3) rotmat(1,2) = 2*(q2q3 - q1q4) rotmat(2,2) = 2*(q1sq + q3sq) - 1.0_rp rotmat(3,2) = 2*(q3q4 + q1q2) rotmat(1,3) = 2*(q2q4 + q1q3) rotmat(2,3) = 2*(q3q4 - q1q2) rotmat(3,3) = 2*(q1sq + q4sq) - 1.0_rp end subroutine !************************************************************************ subroutine quat_shiftmat(q, forward, shiftmat) !! Returns the shifter matrix corresponding to a unit quaternion. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. logical, intent(in) :: forward !! Whether to shift forward, i.e., along the orientation or !! shift reverse. real(rp), dimension(3,3), intent(out) :: shiftmat !! Shifter matrix. real(rp), dimension(4) :: conj_q if (forward) then conj_q = q call quat_conjugated(conj_q) call quat_rotmat(conj_q, shiftmat) else call quat_rotmat(q, shiftmat) end if end subroutine !************************************************************************ subroutine quat_rotate_vectors(v, q, vrot) !! Rotates vectors by a unit quaternion. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to rotate. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(:,:), intent(out) :: vrot !! *(3,n)* Rotated vectors. real(rp), dimension(3,3) :: rotmat call quat_rotmat(q, rotmat) vrot = matmul(rotmat, v) end subroutine !************************************************************************ subroutine quat_shift_vectors(v, q, forward, vshift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by a unit quaternion, shifts vectors from A !! to B or B to A. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to shift. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. logical, intent(in) :: forward !! If `.true.`, shift from A to B. If `.false.`, shift from B to A. real(rp), dimension(:,:), intent(out) :: vshift !! *(3,n)* Shifted vectors. real(rp), dimension(3,3) :: shiftmat call quat_shiftmat(q, forward, shiftmat) vshift = matmul(shiftmat, v) end subroutine !************************************************************************ subroutine quat_shift_tensor2(a, q, forward, ashift) !! Given two frames A and B such that the orientation of frame B with respect !! to frame A is given by a unit quaternion, shifts second-order tensors from A !! to B or B to A. real(rp), dimension(3,3), intent(in) :: a !! A second-order tensor. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. logical, intent(in) :: forward !! If `.true.`, shift from A to B. If `.false.`, shift from B to A. real(rp), dimension(3,3), intent(out) :: ashift !! Shifted second-order tensor. real(rp), dimension(3,3) :: shiftmat call quat_shiftmat(q, forward, shiftmat) call shift_tensor2(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine quat_shift_tensor3(a, q, forward, ashift) !! Given two frames A and B such that the orientation of frame B with respect !! to frame A is given by a unit quaternion, shifts third-order tensors from A !! to B or B to A. real(rp), dimension(3,3,3), intent(in) :: a !! A third-order tensor. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. logical, intent(in) :: forward !! If `.true.`, shift from A to B. If `.false.`, shift from B to A. real(rp), dimension(3,3,3), intent(out) :: ashift !! Shifted third-order tensor. real(rp), dimension(3,3) :: shiftmat call quat_shiftmat(q, forward, shiftmat) call shift_tensor3(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine quat_to_aa (q, axis, angle) !! Converts a unit quaternion to an *axis-angle* representation. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(3), intent(out) :: axis !! Unit vector along the axis of rotation. real(rp), intent(out) :: angle !! Angle in radian. real(rp), dimension(3,3) :: rotmat(3,3) real(rp) :: sine angle = 2*acos(q(1)) sine = sqrt(1.0_rp-q(1)*q(1)) if (angle > 0.0_rp) then if (angle < math_pi) then axis = q(2:4)/sine else call quat_rotmat(q, rotmat) call aa_from_rotmat(rotmat, axis, angle) end if else axis = [1.0_rp, 0.0_rp, 0.0_rp] end if call aa_fix(axis, angle, .true.) end subroutine !************************************************************************ subroutine quat_to_dcm (q, dcm) !! Converts a unit quaternion to a direction cosine matrix. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. real(rp), dimension(3,3), intent(out) :: dcm !! Direction cosine matrix. call quat_shiftmat(q, .true., dcm) end subroutine !************************************************************************ subroutine quat_to_euler (q, seq, world, euler) !! Converts a unit quaternion to an Euler angle sequence. real(rp), dimension(4), intent(in) :: q !! Unit quaternion. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3), intent(out) :: euler !! Euler angles. real(rp), dimension(3,3) :: rotmat call quat_rotmat(q, rotmat) call factor_rotmat(rotmat, seq, world, euler) end subroutine !*****************************************************************************! ! AXIS ANGLE ! !*****************************************************************************! subroutine aa_fix (axis, angle, normalize) !! Modifies `axis` and `angle` to ensure a right handed rotation with !! `angle` \(\in [0, \pi)\). real(rp), dimension(3), intent(in out) :: axis !! Axis of rotation. If this is not a unit vector, set `normalize` to !! ``True``. real(rp), intent(in out) :: angle !! Angle in radian. logical, intent(in), optional :: normalize !! Whether to normalize the axis to a unit vector. logical :: normalize_ real(rp) :: norm normalize_ = .true. if (present(normalize)) normalize_ = normalize if (normalize_) call unitize(axis) angle = mod(angle, 2*math_pi) if (angle < 0.0_rp) then angle = -angle axis = -axis end if if (angle > math_pi) then angle = 2*math_pi - angle axis = -axis end if end subroutine !************************************************************************ subroutine aa_rand (axis, angle) !! Generates a random orientation in *axis-angle* representation. !! The axis is a random vector drawn from a uniform distribution on the !! surface of a unit sphere. The current implementation in based on the !! algorithm from Allen & Tildesley p. 349. real(rp), dimension(3), intent(out) :: axis real(rp), intent(out) :: angle real(rp) :: zeta1, zeta2, zetasq real(rp) :: rt axis = 0.0_rp !Generate angle: A uniform random number from [0, 2*pi) call random_number(angle) angle = 2*math_pi*angle do !Generate two uniform random numbers from [-1, 1) call random_number(zeta1) call random_number(zeta2) zeta1 = 2*zeta1 - 1.0_rp zeta2 = 2*zeta2 - 1.0_rp zetasq = zeta1**2 + zeta2**2 if (zetasq <= 1._rp) exit end do rt = sqrt(1.0_rp-zetasq) axis(1) = 2*zeta1*rt axis(2) = 2*zeta2*rt axis(3) = 1.0_rp - 2*zetasq call aa_fix(axis, angle, .true.) end subroutine !************************************************************************ subroutine aa_rotmat (axis, angle, rotmat) !! Returns the rotation matrix corresponding to an *axis-angle* !! representation. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. real(rp), dimension(3,3), intent(out) :: rotmat !! Rotation matrix. real(rp) :: sine, cosine, icos sine = sin(angle) cosine = cos(angle) icos = 1.0_rp - cosine rotmat(1,1) = axis(1)*axis(1)*icos + cosine rotmat(2,1) = axis(1)*axis(2)*icos + axis(3)*sine rotmat(3,1) = axis(3)*axis(1)*icos - axis(2)*sine rotmat(1,2) = axis(1)*axis(2)*icos - axis(3)*sine rotmat(2,2) = axis(2)*axis(2)*icos + cosine rotmat(3,2) = axis(2)*axis(3)*icos + axis(1)*sine rotmat(1,3) = axis(1)*axis(3)*icos + axis(2)*sine rotmat(2,3) = axis(2)*axis(3)*icos - axis(1)*sine rotmat(3,3) = axis(3)*axis(3)*icos + cosine end subroutine !************************************************************************ subroutine aa_from_rotmat (rotmat, axis, angle) !! Extracts axis and angle from a rotation matrix. real(rp), dimension(3,3), intent(in) :: rotmat !! Rotation matrix (must be orthonormal). real(rp), dimension(3), intent(out) :: axis !! Unit vector along the axis. real(rp), intent(out) :: angle !! Angle of rotation in radian. real(rp) :: tr real(rp) :: s integer :: k tr = rotmat(1,1) + rotmat(2,2) + rotmat(3,3) angle = acos((tr-1.0_rp)/2) if (angle > 0.0_rp) then if (angle < math_pi) then axis(1) = rotmat(3,2) - rotmat(2,3) axis(2) = rotmat(1,3) - rotmat(3,1) axis(3) = rotmat(2,1) - rotmat(1,2) else !Find the largest entry in the diagonal of rotmat k = maxloc([rotmat(1,1), rotmat(2,2), rotmat(3,3)], 1) select case (k) case (1) axis(1) = sqrt(rotmat(1,1)-rotmat(2,2)-rotmat(3,3)+1.0_rp)/2 s = 1.0_rp/(2*axis(1)) axis(2) = s*rotmat(1,2) axis(3) = s*rotmat(1,3) case (2) axis(2) = sqrt(rotmat(2,2)-rotmat(1,1)-rotmat(3,3)+1.0_rp)/2 s = 1.0_rp/(2*axis(2)) axis(1) = s*rotmat(1,2) axis(3) = s*rotmat(2,3) case (3) axis(3) = sqrt(rotmat(3,3)-rotmat(1,1)-rotmat(2,2)+1.0_rp)/2 s = 1.0_rp/(2*axis(3)) axis(1) = s*rotmat(1,3) axis(2) = s*rotmat(2,3) end select end if else axis = [1.0_rp, 0.0_rp, 0.0_rp] end if !Ensuring that angle lies in [0, pi) call aa_fix(axis, angle, .true.) end subroutine !************************************************************************ subroutine aa_rotate_vectors (v, axis, angle, vrot) !! Rotates vectors about `axis` by `angle`. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to rotate. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. real(rp), dimension(:,:), intent(out) :: vrot !! *(3,n)*. Rotated vectors. real(rp), dimension(3,3) :: rotmat call aa_rotmat(axis, angle, rotmat) vrot = matmul(rotmat, v) end subroutine !************************************************************************ subroutine aa_shiftmat (axis, angle, forward, shiftmat) !! Returns the shifter matrix corresponding to an *axis-angle* !! representation. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. logical, intent(in) :: forward !! Whether to shift forward, i.e., along the orientation or shift reverse. real(rp), dimension(3,3), intent(out) :: shiftmat !! Shifter matrix. !Get shiftmat assuming forward == .true. call aa_rotmat(-axis, angle, shiftmat) if (.not. forward) then shiftmat = transpose(shiftmat) end if end subroutine !************************************************************************ subroutine aa_shift_vectors (v, axis, angle, forward, vshift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by an *axis-angle* representation, shifts !! vectors from A to B or B to A. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to shift. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. logical, intent(in) :: forward !! If ``True``, shift from A to B. If ``False``, shift from B to A. real(rp), dimension(:,:), intent(out) :: vshift !! *(3,n)*. Shifted vectors. real(rp), dimension(3,3) :: shiftmat call aa_shiftmat(axis, angle, forward, shiftmat) vshift = matmul(shiftmat, v) end subroutine !************************************************************************ subroutine aa_shift_tensor2 (a, axis, angle, forward, ashift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by an axis-angle representation, shifts !! second-order tensors from A to B or B to A. real(rp), dimension(3,3), intent(in) :: a !! Second-order tensor real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. logical, intent(in) :: forward !! If ``True``, shift from A to B. If ``False``, shift from B to A. real(rp), dimension(3,3), intent(out) :: ashift !! Shifted second-order tensor. real(rp), dimension(3,3) :: shiftmat call aa_shiftmat(axis, angle, forward, shiftmat) call shift_tensor2(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine aa_shift_tensor3 (a, axis, angle, forward, ashift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by an axis-angle representation, shifts !! third-order tensors from A to B or B to A. real(rp), dimension(3,3,3), intent(in) :: a !! Third-order tensor real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. logical, intent(in), optional :: forward !! If ``True``, shift from A to B. If ``False``, shift from B to A. real(rp), dimension(3,3,3), intent(out) :: ashift !! Shifted third-order tensor. real(rp), dimension(3,3) :: shiftmat call aa_shiftmat(axis, angle, forward, shiftmat) call shift_tensor3(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine aa_to_quat(axis, angle, q) !! Converts an *axis-angle* representation to a unit quaternion. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. real(rp), dimension(4), intent(out) :: q !! Unit quaternion. q(1) = cos(angle/2) q(2:4) = sin(angle/2)*axis !Normalize to ensure unit quaternion call quat_normalized(q) end subroutine !************************************************************************ subroutine aa_to_dcm (axis, angle, dcm) !! Converts an *axis-angle* representation to a direction cosine matrix. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle of rotation in radian. `0 <= angle < pi`. real(rp), dimension(3,3), intent(out) :: dcm !! Direction cosine matrix. call aa_shiftmat(axis, angle, .true., dcm) end subroutine !************************************************************************ subroutine aa_to_euler (axis, angle, seq, world, euler) !! Coverts an *axis-angle* representation to *Euler angles*. real(rp), dimension(3), intent(in) :: axis !! Unit vector along the axis. real(rp), intent(in) :: angle !! Angle in radian. `0 <= angle < pi`. character(len=3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3), intent(out) :: euler !! Euler angles. real(rp), dimension(3,3) :: rotmat call aa_rotmat(axis, angle, rotmat) call factor_rotmat(rotmat, seq, world, euler) end subroutine !*****************************************************************************! ! DIRECTION COSINE MATRIX ! !*****************************************************************************! logical function mat_is_dcm (mat) !! Returns *true* if a (3,3) matrix is a direction cosine matrix. real(rp), dimension(3,3), intent(in) :: mat !! Matrix to check. mat_is_dcm = mat_is_rotmat(mat) end function !************************************************************************ subroutine dcm_from_axes (A, B, dcm) !! Returns the direction cosine matrix of axes(i.e. frame) B with !! respect to axes(i.e. frame) A. real(rp), dimension(3,3), intent(in) :: A !! The columns of A represent the orthonormal basis vectors of frame A. real(rp), dimension(3,3), intent(in) :: B !! The columns of B represent the orthonormal basis vectors of frame B. real(rp), dimension(3,3), intent(out) :: dcm !! Direction cosine matrix of frame B with respect to frame A dcm = matmul(transpose(A), B) end subroutine !************************************************************************ subroutine dcm_rotmat (dcm, rotmat) !! Returns the rotation matrix corresponding to a direction cosine matrix. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. real(rp), dimension(3,3), intent(out) :: rotmat !! Rotation matrix. rotmat = transpose(dcm) end subroutine !************************************************************************ subroutine dcm_shiftmat (dcm, forward, shiftmat) !! Returns the shifter matrix corresponding to a direction cosine matrix. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. logical, intent(in) :: forward !! Whether to shift forward, i.e., along the orientation or shift reverse. real(rp), dimension(3,3), intent(out) :: shiftmat !! Shifter matrix. if (forward) then shiftmat = dcm else shiftmat = transpose(shiftmat) end if end subroutine !************************************************************************ subroutine dcm_rotate_vectors (v, dcm, vrot) !! Rotates vectors by a direction cosine matrix. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to rotate. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. real(rp), dimension(:,:), intent(out) :: vrot !! *(3,n)*. Rotated vectors. real(rp) :: rotmat(3,3) call dcm_rotmat(dcm, rotmat) vrot = matmul(rotmat, v) end subroutine !************************************************************************ subroutine dcm_shift_vectors (v, dcm, forward, vshift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by a direction cosine matrix, shifts !! vectors from A to B or B to A. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to shift. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. logical, intent(in) :: forward !! If ``True``, shift from A to B. If ``False``, shift from B to A. real(rp), dimension(:,:), intent(out) :: vshift !! *(3,n)*. Shifted vectors. real(rp), dimension(3,3) :: shiftmat call dcm_shiftmat(dcm, forward, shiftmat) vshift = matmul(shiftmat, v) end subroutine !************************************************************************ subroutine dcm_shift_tensor2 (a, dcm, forward, ashift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by a direction cosine matrix, shifts !! second-order tesnors from A to B or B to A. real(rp), dimension(3,3), intent(in) :: a !! A second-order tensor. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. logical, intent(in) :: forward !! If ``True``, shift from A to B. If ``False``, shift from B to A. real(rp), dimension(3,3), intent(out) :: ashift !! *(3,n)*. Shifted second-order tensor. real(rp), dimension(3,3) :: shiftmat call dcm_shiftmat(dcm, forward, shiftmat) call shift_tensor2(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine dcm_shift_tensor3 (a, dcm, forward, ashift) !! Given two frames A and B such that the orientation of frame B with !! respect to frame A is given by a direction cosine matrix, shifts !! third-order tesnors from A to B or B to A. real(rp), dimension(3,3,3), intent(in) :: a !! A second-order tensor. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. logical, intent(in) :: forward !! If ``True``, shift from A to B. If ``False``, shift from B to A. real(rp), dimension(3,3,3), intent(out) :: ashift !! *(3,n)*. Shifted second-order tensor. real(rp), dimension(3,3) :: shiftmat call dcm_shiftmat(dcm, forward, shiftmat) call shift_tensor3(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine dcm_to_quat (dcm, q) !! Converts a direction cosine matrix to a unit quaternion. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. real(rp), dimension(4), intent(out) :: q !! Unit quaternion. real(rp), dimension(3,3) :: rotmat real(rp), dimension(3) :: axis(3) real(rp) :: angle call dcm_rotmat(dcm, rotmat) call aa_from_rotmat(rotmat, axis, angle) call aa_to_quat(axis, angle, q) end subroutine !************************************************************************ subroutine dcm_to_aa (dcm, axis, angle) !! Converts a direction cosine matrix to an *axis-angle* representation. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. real(rp), dimension(3), intent(out) :: axis !! Unit vector along the axis. real(rp), intent(out) :: angle !! Angle in radian. real(rp), dimension(3,3) :: rotmat call dcm_rotmat(dcm, rotmat) call aa_from_rotmat(rotmat, axis, angle) end subroutine !************************************************************************ subroutine dcm_to_euler (dcm, seq, world, euler) !! Converts a direction cosine matrix to Euler angles. real(rp), dimension(3,3), intent(in) :: dcm !! Direction cosine matrix. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3), intent(out) :: euler !! Euler angles real(rp), dimension(3,3) :: rotmat call dcm_rotmat (dcm, rotmat) call factor_rotmat(rotmat, seq, world, euler) end subroutine !*****************************************************************************! ! EULER ANGLES ! !*****************************************************************************! subroutine euler_rotmat (euler, seq, world, rotmat) !! Returns the rotation matrix for a set of Euler angles. real(rp), dimension(3), intent(in) :: euler character(len=3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3,3), intent(out) :: rotmat !! Rotation matrix. real(rp), dimension(3) :: euler_ if (.not. world) then euler_ = -euler else euler_ = euler end if select case (seq) case ('XYZ') call rotmat_xyz(euler_, rotmat) case ('XZY') call rotmat_xzy(euler_, rotmat) case ('YXZ') call rotmat_yxz(euler_, rotmat) case ('YZX') call rotmat_yzx(euler_, rotmat) case ('ZXY') call rotmat_zxy(euler_, rotmat) case ('ZYX') call rotmat_zyx(euler_, rotmat) case default stop 'unknown value of seq' end select if (.not. world) rotmat = transpose(rotmat) end subroutine !******************************************************************************* subroutine euler_shiftmat (euler, seq, world, forward, shiftmat) !! Returns the shifter matrix for a set of Euler angles. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. logical, intent(in) :: forward !! Whether to shift forward, i.e., along the orientation or shift reverse. real(rp), dimension(3,3), intent(out) :: shiftmat !! Shifter matrix. real(rp), dimension(3,3) :: rotmat call euler_rotmat(euler, seq, world, rotmat) if (forward) then shiftmat = transpose(rotmat) else shiftmat = rotmat end if end subroutine !************************************************************************ subroutine euler_rotate_vectors (v, euler, seq, world, vrot) !! Rotates vectors with a set of Euler angles. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to rotate. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(:,:), intent(out) :: vrot !! *(3,n)*. Rotated vectors. real(rp), dimension(3,3) :: rotmat call euler_rotmat (euler, seq, world, rotmat) vrot = matmul(rotmat, v) end subroutine !************************************************************************ subroutine euler_shift_vectors (v, euler, seq, world, forward, vshift) !! Given two frames A and B such that the orientation of frame B !! with respect to frame A is given by three Euler angles, shifts !! vectors from A to B or B to A. real(rp), dimension(:,:), intent(in) :: v !! *(3,n)*. Vectors to rotate. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. logical, intent(in) :: forward !! If `.true.`, shift from A to B. If `.false.`, shift from B to A. real(rp), dimension(:,:), intent(out) :: vshift !! *(3,n)*. Shifted vectors. real(rp), dimension(3,3) :: shiftmat call euler_shiftmat (euler, seq, world, forward, shiftmat) vshift = matmul(shiftmat, v) end subroutine !************************************************************************ subroutine euler_shift_tensor2 (a, euler, seq, world, forward, ashift) !! Given two frames A and B such that the orientation of frame B !! with respect to frame A is given by three Euler angles, shifts !! second-order tensors from A to B or B to A. real(rp), dimension(3,3), intent(in) :: a !! Second-order tensor. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. logical, intent(in) :: forward !! If `.true.`, shift from A to B. If `.false.`, shift from B to A. real(rp), dimension(3,3), intent(out) :: ashift !! Shifted second-order tensor. real(rp), dimension(3,3) :: shiftmat call euler_shiftmat (euler, seq, world, forward, shiftmat) call shift_tensor2(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine euler_shift_tensor3 (a, euler, seq, world, forward, ashift) !! Given two frames A and B such that the orientation of frame B !! with respect to frame A is given by three Euler angles, shifts !! third-order tensors from A to B or B to A. real(rp), dimension(3,3,3), intent(in) :: a !! Third-order tensor. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. logical, intent(in) :: forward !! If `.true.`, shift from A to B. If `.false.`, shift from B to A. real(rp), dimension(3,3,3), intent(out) :: ashift !! Shifted third-order tensor. real(rp), dimension(3,3) :: shiftmat call euler_shiftmat (euler, seq, world, forward, shiftmat) call shift_tensor3(a, shiftmat, ashift) end subroutine !************************************************************************ subroutine euler_to_quat (euler, seq, world, q) !! Convert Euler angles to a unit quaternion. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(4), intent(out) :: q !! Unit quaternion. real(rp), dimension(3) :: axis real(rp) :: angle call euler_to_aa(euler, seq, world, axis, angle) call aa_to_quat(axis, angle, q) end subroutine !************************************************************************ subroutine euler_to_aa (euler, seq, world, axis, angle) !! Convert Euler angles to an *axis-angle* representation. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3), intent(out) :: axis !! Unit vector along the axis of rotation. real(rp), intent(out) :: angle !! Angle of rotation in radian. real(rp), dimension(3,3) :: rotmat call euler_rotmat (euler, seq, world, rotmat) call aa_from_rotmat(rotmat, axis, angle) end subroutine !************************************************************************ subroutine euler_to_dcm (euler, seq, world, dcm) !! Convert Euler angles to a direction cosine matrix. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3,3), intent(out) :: dcm !! Direction cosine matrix. real(rp), dimension(3,3) :: rotmat call euler_shiftmat (euler, seq, world, .true., dcm) end subroutine !************************************************************************ subroutine euler_to_euler (euler, seq, world, to_seq, to_world, to_euler) !! Convert one set of Euler angles to another. real(rp), dimension(3), intent(in) :: euler !! Euler angles. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Sequence for `euler`. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. character(3), intent(out) :: to_seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Convert `euler` with sequence `seq` to the sequence `to_seq`. logical, intent(out) :: to_world !! Whether the converted euler angles are with respect to the *world* frame or not. real(rp), dimension(3), intent(out) :: to_euler !! Converted Euler angles. real(rp), dimension(3,3) :: rotmat call euler_rotmat (euler, seq, world, rotmat) call factor_rotmat(rotmat, to_seq, to_world, to_euler) end subroutine !*****************************************************************************! ! INTERNAL ROUTINES ! !*****************************************************************************! subroutine factor_rotmat (rotmat, seq, world, euler) !! Factorize a rotation matrix to obtain the three Euler angles. real(rp), dimension(3,3), intent(in) :: rotmat !! Rotation matrix. character(3), intent(in) :: seq !! `'XYZ'`|`'XZY'` | `'YXZ'` | `'YZX'` | `'ZXY'` | `'ZYX'`. !! Euler angle sequence. logical, intent(in) :: world !! Whether the Euler angles are with respect to the *world* frame or not. real(rp), dimension(3), intent(out) :: euler !! Euler angles. real(rp), dimension(3,3) :: rotmat_ if (.not. world) then rotmat_ = transpose(rotmat) else rotmat_ = rotmat end if select case (seq) case ('XYZ') call factor_rotmat_xyz(rotmat_, euler) case ('XZY') call factor_rotmat_xzy(rotmat_, euler) case ('YXZ') call factor_rotmat_yxz(rotmat_, euler) case ('YZX') call factor_rotmat_yzx(rotmat_, euler) case ('ZXY') call factor_rotmat_zxy(rotmat_, euler) case ('ZYX') call factor_rotmat_zyx(rotmat_, euler) case default stop 'Unknown value of seq' end select if (.not. world) euler = -euler end subroutine !************************************************************************ subroutine shift_tensor2(a, shiftmat, ashift) !! Shifts a second order tensor, i.e. (3,3) matrix using a shifter matrix real(rp), dimension(3,3), intent(in) :: a real(rp), dimension(3,3), intent(in) :: shiftmat real(rp), dimension(3,3), intent(out) :: ashift ashift = matmul(shiftmat, matmul(a, transpose(shiftmat))) end subroutine !************************************************************************ subroutine shift_tensor3(a, shiftmat, ashift) !! Shifts a third order tensor, i.e. (3,3,3) matrix using a shifter matrix real(rp), dimension(3,3,3), intent(in) :: a real(rp), dimension(3,3), intent(in) :: shiftmat real(rp), dimension(3,3,3), intent(out) :: ashift integer :: i, j, k, p, q, r ashift = 0.0_rp do r = 1, 3 do q = 1, 3 do p = 1, 3 do k = 1, 3 do j = 1, 3 do i = 1, 3 ashift(i,j,k) = ashift(i,j,k) + & shiftmat(i,p)*shiftmat(j,q)*shiftmat(k,r)*a(p,q,r) end do end do end do end do end do end do end subroutine !******************************************************************************* subroutine rotmat_xyz(euler, rotmat) real(rp), dimension(3), intent(in) :: euler real(rp), dimension(3,3), intent(out) :: rotmat real(rp) :: phi, theta, psi real(rp) :: sin_phi, sin_theta, sin_psi real(rp) :: cos_phi, cos_theta, cos_psi rotmat = 0.0_rp phi = euler(1); theta = euler(2); psi = euler(3) sin_phi = sin(phi); sin_theta = sin(theta); sin_psi = sin(psi) cos_phi = cos(phi); cos_theta = cos(theta); cos_psi = cos(psi) rotmat(1,1) = cos_theta*cos_psi rotmat(2,1) = cos_theta*sin_psi rotmat(3,1) = -sin_theta rotmat(1,2) = sin_phi*sin_theta*cos_psi - cos_phi*sin_psi rotmat(2,2) = sin_psi*sin_theta*sin_phi + cos_phi*cos_psi rotmat(3,2) = sin_phi*cos_theta rotmat(1,3) = cos_phi*sin_theta*cos_psi + sin_phi*sin_psi rotmat(2,3) = cos_phi*sin_theta*sin_psi - sin_phi*cos_psi rotmat(3,3) = cos_phi*cos_theta end subroutine !******************************************************************************* subroutine rotmat_xzy(euler, rotmat) real(rp), dimension(3), intent(in) :: euler real(rp), dimension(3,3), intent(out) :: rotmat real(rp) :: phi, theta, psi real(rp) :: sin_phi, sin_theta, sin_psi real(rp) :: cos_phi, cos_theta, cos_psi rotmat = 0.0_rp phi = euler(1); theta = euler(2); psi = euler(3) sin_phi = sin(phi); sin_theta = sin(theta); sin_psi = sin(psi) cos_phi = cos(phi); cos_theta = cos(theta); cos_psi = cos(psi) rotmat(1,1) = cos_theta*cos_psi rotmat(2,1) = sin_psi rotmat(3,1) = -sin_theta*cos_psi rotmat(1,2) = sin_phi*sin_theta - cos_phi*cos_theta*sin_psi rotmat(2,2) = cos_phi*cos_psi rotmat(3,2) = sin_phi*cos_theta + cos_phi*sin_theta*sin_psi rotmat(1,3) = cos_phi*sin_theta + sin_phi*cos_theta*sin_psi rotmat(2,3) = -sin_phi*cos_psi rotmat(3,3) = cos_phi*cos_theta - sin_phi*sin_theta*sin_psi end subroutine !******************************************************************************* subroutine rotmat_yxz(euler, rotmat) real(rp), dimension(3), intent(in) :: euler real(rp), dimension(3,3), intent(out) :: rotmat real(rp) :: phi, theta, psi real(rp) :: sin_phi, sin_theta, sin_psi real(rp) :: cos_phi, cos_theta, cos_psi rotmat = 0.0_rp phi = euler(1); theta = euler(2); psi = euler(3) sin_phi = sin(phi); sin_theta = sin(theta); sin_psi = sin(psi) cos_phi = cos(phi); cos_theta = cos(theta); cos_psi = cos(psi) rotmat(1,1) = cos_theta*cos_psi - sin_phi*sin_theta*sin_psi rotmat(2,1) = sin_phi*sin_theta*cos_psi + cos_theta*sin_psi rotmat(3,1) = -cos_phi*sin_theta rotmat(1,2) = -cos_phi*sin_psi rotmat(2,2) = cos_phi*cos_psi rotmat(3,2) = sin_phi rotmat(1,3) = sin_theta*cos_psi + sin_phi*cos_theta*sin_psi rotmat(2,3) = sin_theta*sin_psi - sin_phi*cos_theta*cos_psi rotmat(3,3) = cos_phi*cos_theta end subroutine !******************************************************************************* subroutine rotmat_yzx(euler, rotmat) real(rp), dimension(3), intent(in) :: euler real(rp), dimension(3,3), intent(out) :: rotmat real(rp) :: phi, theta, psi real(rp) :: sin_phi, sin_theta, sin_psi real(rp) :: cos_phi, cos_theta, cos_psi rotmat = 0.0_rp phi = euler(1); theta = euler(2); psi = euler(3) sin_phi = sin(phi); sin_theta = sin(theta); sin_psi = sin(psi) cos_phi = cos(phi); cos_theta = cos(theta); cos_psi = cos(psi) rotmat(1,1) = cos_theta*cos_psi rotmat(2,1) = sin_phi*sin_theta + cos_phi*cos_theta*sin_psi rotmat(3,1) = sin_phi*cos_theta*sin_psi - cos_phi*sin_theta rotmat(1,2) = -sin_psi rotmat(2,2) = cos_phi*cos_psi rotmat(3,2) = sin_phi*cos_psi rotmat(1,3) = sin_theta*cos_psi rotmat(2,3) = cos_phi*sin_theta*sin_psi - sin_phi*cos_theta rotmat(3,3) = sin_phi*sin_theta*sin_psi + cos_phi*cos_theta end subroutine !******************************************************************************* subroutine rotmat_zxy(euler, rotmat) real(rp), dimension(3), intent(in) :: euler real(rp), dimension(3,3), intent(out) :: rotmat real(rp) :: phi, theta, psi real(rp) :: sin_phi, sin_theta, sin_psi real(rp) :: cos_phi, cos_theta, cos_psi rotmat = 0.0_rp phi = euler(1); theta = euler(2); psi = euler(3) sin_phi = sin(phi); sin_theta = sin(theta); sin_psi = sin(psi) cos_phi = cos(phi); cos_theta = cos(theta); cos_psi = cos(psi) rotmat(1,1) = cos_theta*cos_psi + sin_phi*sin_theta*sin_psi rotmat(2,1) = cos_phi*sin_psi rotmat(3,1) = sin_phi*cos_theta*sin_psi - sin_theta*cos_psi rotmat(1,2) = sin_phi*sin_theta*cos_psi - cos_theta*sin_psi rotmat(2,2) = cos_phi*cos_psi rotmat(3,2) = sin_phi*cos_theta*cos_psi + sin_theta*sin_psi rotmat(1,3) = cos_phi*sin_theta rotmat(2,3) = -sin_phi rotmat(3,3) = cos_phi*cos_theta end subroutine !******************************************************************************* subroutine rotmat_zyx (euler, rotmat) real(rp), dimension(3), intent(in) :: euler real(rp), dimension(3,3), intent(out) :: rotmat real(rp) :: phi, theta, psi real(rp) :: sin_phi, sin_theta, sin_psi real(rp) :: cos_phi, cos_theta, cos_psi rotmat = 0.0_rp phi = euler(1); theta = euler(2); psi = euler(3) sin_phi = sin(phi); sin_theta = sin(theta); sin_psi = sin(psi) cos_phi = cos(phi); cos_theta = cos(theta); cos_psi = cos(psi) rotmat(1,1) = cos_theta*cos_psi rotmat(2,1) = sin_phi*sin_theta*cos_psi + cos_phi*sin_psi rotmat(3,1) = sin_phi*sin_psi - cos_phi*sin_theta*cos_psi rotmat(1,2) = -cos_theta*sin_psi rotmat(2,2) = cos_phi*cos_psi - sin_phi*sin_theta*sin_psi rotmat(3,2) = sin_phi*cos_psi + cos_phi*sin_theta*sin_psi rotmat(1,3) = sin_theta rotmat(2,3) = -sin_phi*cos_theta rotmat(3,3) = cos_phi*cos_theta end subroutine !******************************************************************************* subroutine factor_rotmat_xyz (rotmat, euler) real(rp), dimension(3,3), intent(in) :: rotmat real(rp), dimension(3), intent(out) :: euler real(rp) :: phi, theta, psi if (rotmat(3,1) < 1.0_rp) then if (rotmat(3,1) > -1.0_rp) then theta = asin(-rotmat(3,1)) psi = atan2(rotmat(2,1), rotmat(1,1)) phi = atan2(rotmat(3,2), rotmat(3,3)) else !Not unique: phi - psi = atan2(-rotmat(2,3), rotmat(2,2)) theta = math_pi_2 psi = -atan2(-rotmat(2,3), rotmat(2,2)) phi = 0.0_rp end if else !Not unique: phi + psi = atan2(-rotmat(2,3), rotmat(2,2)) phi = 0.0_rp theta = -math_pi_2 psi = atan2(-rotmat(2,3), rotmat(2,2)) end if euler(1) = phi; euler(2) = theta; euler(3) = psi end subroutine !******************************************************************************* subroutine factor_rotmat_xzy (rotmat, euler) real(rp), dimension(3,3), intent(in) :: rotmat real(rp), dimension(3), intent(out) :: euler real(rp) :: phi, theta, psi if (rotmat(2,1) < 1.0_rp) then if (rotmat(2,1) > -1.0_rp) then phi = atan2(-rotmat(2,3), rotmat(2,2)) theta = atan2(-rotmat(3,1), rotmat(1,1)) psi = asin(rotmat(2,1)) else !Not unique: phi - theta = atan2(rotmat(3,2), rotmat(3,3)) phi = 0.0_rp theta = -atan2(rotmat(3,2), rotmat(3,3)) psi = -math_pi_2 end if else !Not unique: phi + theta = atan2(rotmat(3,2), rotmat(3,3)) phi = 0.0_rp theta = atan2(rotmat(3,2), rotmat(3,3)) psi = math_pi_2 end if euler(1) = phi; euler(2) = theta; euler(3) = psi end subroutine !******************************************************************************* subroutine factor_rotmat_yxz (rotmat, euler) real(rp), dimension(3,3), intent(in) :: rotmat real(rp), dimension(3), intent(out) :: euler real(rp) :: phi, theta, psi if (rotmat(3,2) < 1.0_rp) then if (rotmat(3,2) > -1.0_rp) then phi = asin(rotmat(3,2)) theta = atan2(-rotmat(3,1), rotmat(3,3)) psi = atan2(-rotmat(1,2), rotmat(2,2)) else !Not unique: theta - psi = atan2(rotmat(1,3), rotmat(1,1)) phi = -math_pi_2 theta = 0.0_rp psi = -atan2(rotmat(1,3), rotmat(1,1)) end if else !Not unique: theta + psi = atan2(rotmat(1,3), rotmat(1,1)) phi = math_pi_2 theta = 0.0_rp psi = atan2(rotmat(1,3), rotmat(1,1)) end if euler(1) = phi; euler(2) = theta; euler(3) = psi end subroutine !******************************************************************************* subroutine factor_rotmat_yzx (rotmat, euler) real(rp), dimension(3,3), intent(in) :: rotmat real(rp), dimension(3), intent(out) :: euler real(rp) :: phi, theta, psi if (rotmat(1,2) < 1.0_rp) then if (rotmat(1,2) > -1.0_rp) then phi = atan2(rotmat(3,2), rotmat(2,2)) theta = atan2(rotmat(1,3), rotmat(1,1)) psi = asin(-rotmat(1,2)) else !Not unique: theta - phi = atan2(-rotmat(3,1), rotmat(3,3)) phi = -atan2(-rotmat(3,1), rotmat(3,3)) theta = 0.0_rp psi = math_pi_2 end if else !Not unique: theta + phi = atan2(-rotmat(3,1), rotmat(3,3)) phi = atan2(-rotmat(3,1), rotmat(3,3)) theta = 0.0_rp psi = -math_pi_2 end if euler(1) = phi; euler(2) = theta; euler(3) = psi end subroutine !******************************************************************************* subroutine factor_rotmat_zxy (rotmat, euler) real(rp), dimension(3,3), intent(in) :: rotmat real(rp), dimension(3), intent(out) :: euler real(rp) :: phi, theta, psi if (rotmat(2,3) < 1.0_rp) then if (rotmat(2,3) > -1.0_rp) then phi = asin(-rotmat(2,3)) theta = atan2(rotmat(1,3), rotmat(3,3)) psi = atan2(rotmat(2,1), rotmat(2,2)) else !Not unique: psi - theta = atan2(-rotmat(1,2), rotmat(1,1)) phi = math_pi_2 theta = -atan2(-rotmat(1,2), rotmat(1,1)) psi = 0.0_rp end if else !Not unique: psi + theta = atan2(-rotmat(1,2), rotmat(1,1)) phi = -math_pi_2 theta = atan2(-rotmat(1,2), rotmat(1,1)) psi = 0.0_rp end if euler(1) = phi; euler(2) = theta; euler(3) = psi end subroutine !******************************************************************************* subroutine factor_rotmat_zyx (rotmat, euler) real(rp), dimension(3,3), intent(in) :: rotmat real(rp), dimension(3), intent(out) :: euler real(rp) :: phi, theta, psi if (rotmat(1,3) < 1.0_rp) then if (rotmat(1,3) > -1.0_rp) then phi = atan2(-rotmat(2,3), rotmat(3,3)) theta = asin(rotmat(1,3)) psi = atan2(-rotmat(1,2), rotmat(1,1)) else !Not unique: psi - phi = atan2(rotmat(2,1), rotmat(2,2)) phi = -atan2(rotmat(2,1), rotmat(2,2)) theta = -math_pi_2 psi = 0.0_rp end if else !Not unique: psi + phi = atan2(rotmat(2,1), rotmat(2,2)) phi = atan2(rotmat(2,1), rotmat(2,2)) theta = math_pi_2 psi = 0.0_rp end if euler(1) = phi; euler(2) = theta; euler(3) = psi end subroutine !****************************************************************************** end module rotlib_m