for comparison with de-facto stardard rotation definitions

This commit is contained in:
Martin Diehl 2017-10-07 13:18:42 +02:00
parent 1612343065
commit e32b9d9ca8
1 changed files with 62 additions and 54 deletions

View File

@ -1384,35 +1384,37 @@ end function math_RtoQ
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
!> @brief rotation matrix from Euler angles (in radians) !> @brief rotation matrix from Bunge-Euler (3-1-3) angles (in radians)
!> @details rotation matrix is meant to represent a PASSIVE rotation, !> @details rotation matrix is meant to represent a PASSIVE rotation, composed of INTRINSIC
!> @details composed of INTRINSIC rotations around the axes of the !> @details rotations around the axes of the details rotating reference frame.
!> @details rotating reference frame !> @details similar to eu2om from "D Rowenhorst et al. Consistent representations of and conversions
!> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions) !> @details between 3D rotations, Model. Simul. Mater. Sci. Eng. 23-8 (2015)", but R is transposed
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
pure function math_EulerToR(Euler) pure function math_EulerToR(Euler)
implicit none implicit none
real(pReal), dimension(3), intent(in) :: Euler real(pReal), dimension(3), intent(in) :: Euler
real(pReal), dimension(3,3) :: math_EulerToR real(pReal), dimension(3,3) :: math_EulerToR
real(pReal) c1, c, c2, s1, s, s2 real(pReal) :: c1, C, c2, s1, S, s2
C1 = cos(Euler(1)) c1 = cos(Euler(1))
C = cos(Euler(2)) C = cos(Euler(2))
C2 = cos(Euler(3)) c2 = cos(Euler(3))
S1 = sin(Euler(1)) s1 = sin(Euler(1))
S = sin(Euler(2)) S = sin(Euler(2))
S2 = sin(Euler(3)) s2 = sin(Euler(3))
math_EulerToR(1,1)=C1*C2-S1*S2*C math_EulerToR(1,1) = c1*c2 -s1*C*s2
math_EulerToR(1,2)=-C1*S2-S1*C2*C math_EulerToR(1,2) = -c1*s2 -s1*C*c2
math_EulerToR(1,3)=S1*S math_EulerToR(1,3) = s1*S
math_EulerToR(2,1)=S1*C2+C1*S2*C
math_EulerToR(2,2)=-S1*S2+C1*C2*C math_EulerToR(2,1) = s1*c2 +c1*C*s2
math_EulerToR(2,3)=-C1*S math_EulerToR(2,2) = -s1*s2 +c1*C*c2
math_EulerToR(3,1)=S2*S math_EulerToR(2,3) = -c1*S
math_EulerToR(3,2)=C2*S
math_EulerToR(3,3)=C math_EulerToR(3,1) = S*s2
math_EulerToR(3,2) = S*c2
math_EulerToR(3,3) = C
math_EulerToR = transpose(math_EulerToR) ! convert to passive rotation math_EulerToR = transpose(math_EulerToR) ! convert to passive rotation
@ -1420,29 +1422,29 @@ end function math_EulerToR
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
!> @brief quaternion (w+ix+jy+kz) from 3-1-3 Euler angles (in radians) !> @brief quaternion (w+ix+jy+kz) from Bunge-Euler (3-1-3) angles (in radians)
!> @details quaternion is meant to represent a PASSIVE rotation, !> @details rotation matrix is meant to represent a PASSIVE rotation, composed of INTRINSIC
!> @details composed of INTRINSIC rotations around the axes of the !> @details rotations around the axes of the details rotating reference frame.
!> @details rotating reference frame !> @details similar to eu2qu from "D Rowenhorst et al. Consistent representations of and
!> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions) !> @details conversions between 3D rotations, Model. Simul. Mater. Sci. Eng. 23-8 (2015)", but
!> @details Q is conjucated and Q is not reversed for Q(0) < 0.
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
pure function math_EulerToQ(eulerangles) pure function math_EulerToQ(eulerangles)
implicit none implicit none
real(pReal), dimension(3), intent(in) :: eulerangles real(pReal), dimension(3), intent(in) :: eulerangles
real(pReal), dimension(4) :: math_EulerToQ real(pReal), dimension(4) :: math_EulerToQ
real(pReal), dimension(3) :: halfangles real(pReal) :: c, s, sigma, delta
real(pReal) :: c, s
halfangles = 0.5_pReal * eulerangles c = cos(0.5_pReal * eulerangles(2))
s = sin(0.5_pReal * eulerangles(2))
c = cos(halfangles(2)) sigma = 0.5_pReal * (eulerangles(1)+eulerangles(3))
s = sin(halfangles(2)) delta = 0.5_pReal * (eulerangles(1)-eulerangles(3))
math_EulerToQ= [cos(halfangles(1)+halfangles(3)) * c, & math_EulerToQ= [c * cos(sigma), &
cos(halfangles(1)-halfangles(3)) * s, & s * cos(delta), &
sin(halfangles(1)-halfangles(3)) * s, & s * sin(delta), &
sin(halfangles(1)+halfangles(3)) * c ] c * sin(sigma) ]
math_EulerToQ = math_qConj(math_EulerToQ) ! convert to passive rotation math_EulerToQ = math_qConj(math_EulerToQ) ! convert to passive rotation
end function math_EulerToQ end function math_EulerToQ
@ -1453,6 +1455,8 @@ end function math_EulerToQ
!> @details rotation matrix is meant to represent a ACTIVE rotation !> @details rotation matrix is meant to represent a ACTIVE rotation
!> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions) !> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions)
!> @details formula for active rotation taken from http://mathworld.wolfram.com/RodriguesRotationFormula.html !> @details formula for active rotation taken from http://mathworld.wolfram.com/RodriguesRotationFormula.html
!> @details equivalent to eu2om (P=-1) from "D Rowenhorst et al. Consistent representations of and
!> @details conversions between 3D rotations, Model. Simul. Mater. Sci. Eng. 23-8 (2015)"
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
pure function math_axisAngleToR(axis,omega) pure function math_axisAngleToR(axis,omega)
@ -1460,31 +1464,31 @@ pure function math_axisAngleToR(axis,omega)
real(pReal), dimension(3,3) :: math_axisAngleToR real(pReal), dimension(3,3) :: math_axisAngleToR
real(pReal), dimension(3), intent(in) :: axis real(pReal), dimension(3), intent(in) :: axis
real(pReal), intent(in) :: omega real(pReal), intent(in) :: omega
real(pReal), dimension(3) :: axisNrm real(pReal), dimension(3) :: n
real(pReal) :: norm,s,c,c1 real(pReal) :: norm,s,c,c1
norm = norm2(axis) norm = norm2(axis)
if (norm > 1.0e-8_pReal) then ! non-zero rotation wellDefined: if (norm > 1.0e-8_pReal) then
axisNrm = axis/norm ! normalize axis to be sure n = axis/norm ! normalize axis to be sure
s = sin(omega) s = sin(omega)
c = cos(omega) c = cos(omega)
c1 = 1.0_pReal - c c1 = 1.0_pReal - c
math_axisAngleToR(1,1) = c + c1*axisNrm(1)**2.0_pReal math_axisAngleToR(1,1) = c + c1*n(1)**2.0_pReal
math_axisAngleToR(1,2) = -s*axisNrm(3) + c1*axisNrm(1)*axisNrm(2) math_axisAngleToR(1,2) = c1*n(1)*n(2) - s*n(3)
math_axisAngleToR(1,3) = s*axisNrm(2) + c1*axisNrm(1)*axisNrm(3) math_axisAngleToR(1,3) = c1*n(1)*n(3) + s*n(2)
math_axisAngleToR(2,1) = s*axisNrm(3) + c1*axisNrm(2)*axisNrm(1) math_axisAngleToR(2,1) = c1*n(1)*n(2) + s*n(3)
math_axisAngleToR(2,2) = c + c1*axisNrm(2)**2.0_pReal math_axisAngleToR(2,2) = c + c1*n(2)**2.0_pReal
math_axisAngleToR(2,3) = -s*axisNrm(1) + c1*axisNrm(2)*axisNrm(3) math_axisAngleToR(2,3) = c1*n(2)*n(3) - s*n(1)
math_axisAngleToR(3,1) = -s*axisNrm(2) + c1*axisNrm(3)*axisNrm(1) math_axisAngleToR(3,1) = c1*n(1)*n(3) - s*n(2)
math_axisAngleToR(3,2) = s*axisNrm(1) + c1*axisNrm(3)*axisNrm(2) math_axisAngleToR(3,2) = c1*n(2)*n(3) + s*n(1)
math_axisAngleToR(3,3) = c + c1*axisNrm(3)**2.0_pReal math_axisAngleToR(3,3) = c + c1*n(3)**2.0_pReal
else else wellDefined
math_axisAngleToR = math_I3 math_axisAngleToR = math_I3
endif endif wellDefined
end function math_axisAngleToR end function math_axisAngleToR
@ -1493,6 +1497,8 @@ end function math_axisAngleToR
!> @brief rotation matrix from axis and angle (in radians) !> @brief rotation matrix from axis and angle (in radians)
!> @details rotation matrix is meant to represent a PASSIVE rotation !> @details rotation matrix is meant to represent a PASSIVE rotation
!> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions) !> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions)
!> @details eq-uivalent to eu2qu (P=+1) from "D Rowenhorst et al. Consistent representations of and
!> @details conversions between 3D rotations, Model. Simul. Mater. Sci. Eng. 23-8 (2015)"
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
pure function math_EulerAxisAngleToR(axis,omega) pure function math_EulerAxisAngleToR(axis,omega)
@ -1529,8 +1535,10 @@ end function math_EulerAxisAngleToQ
!> @brief quaternion (w+ix+jy+kz) from axis and angle (in radians) !> @brief quaternion (w+ix+jy+kz) from axis and angle (in radians)
!> @details quaternion is meant to represent an ACTIVE rotation !> @details quaternion is meant to represent an ACTIVE rotation
!> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions) !> @details (see http://en.wikipedia.org/wiki/Euler_angles for definitions)
!> @details formula for active rotation taken from !> @details formula for active rotation taken from
!> @details http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rodrigues_parameters !> @details http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rodrigues_parameters
!> @details equivalent to eu2qu (P=+1) from "D Rowenhorst et al. Consistent representations of and
!> @details conversions between 3D rotations, Model. Simul. Mater. Sci. Eng. 23-8 (2015)"
!-------------------------------------------------------------------------------------------------- !--------------------------------------------------------------------------------------------------
pure function math_axisAngleToQ(axis,omega) pure function math_axisAngleToQ(axis,omega)
@ -1541,13 +1549,13 @@ pure function math_axisAngleToQ(axis,omega)
real(pReal), dimension(3) :: axisNrm real(pReal), dimension(3) :: axisNrm
real(pReal) :: norm real(pReal) :: norm
norm = sqrt(math_mul3x3(axis,axis)) norm = norm2(axis)
rotation: if (norm > 1.0e-8_pReal) then wellDefined: if (norm > 1.0e-8_pReal) then
axisNrm = axis/norm ! normalize axis to be sure axisNrm = axis/norm ! normalize axis to be sure
math_axisAngleToQ = [cos(0.5_pReal*omega), sin(0.5_pReal*omega) * axisNrm(1:3)] math_axisAngleToQ = [cos(0.5_pReal*omega), sin(0.5_pReal*omega) * axisNrm(1:3)]
else rotation else wellDefined
math_axisAngleToQ = [1.0_pReal,0.0_pReal,0.0_pReal,0.0_pReal] math_axisAngleToQ = [1.0_pReal,0.0_pReal,0.0_pReal,0.0_pReal]
endif rotation endif wellDefined
end function math_axisAngleToQ end function math_axisAngleToQ