From d69d57221d3cc645b70ae28807408d10163f3979 Mon Sep 17 00:00:00 2001 From: Martin Diehl Date: Fri, 20 Sep 2019 08:36:16 -0700 Subject: [PATCH] consistent type handling and stronger encapsulation components of quaternion are private now qu is an array, not a quaterion (as in the python module). conceptually cleaner because eu,ax,om, etc. are also plain array --- src/quaternions.f90 | 93 +++++++++++++++++++------- src/rotations.f90 | 158 ++++++++++++++++++++++---------------------- 2 files changed, 147 insertions(+), 104 deletions(-) diff --git a/src/quaternions.f90 b/src/quaternions.f90 index a59c5f436..37363ad12 100644 --- a/src/quaternions.f90 +++ b/src/quaternions.f90 @@ -42,10 +42,10 @@ module quaternions real(pReal), parameter, public :: P = -1.0_pReal !< parameter for orientation conversion. type, public :: quaternion - real(pReal) :: w = 0.0_pReal - real(pReal) :: x = 0.0_pReal - real(pReal) :: y = 0.0_pReal - real(pReal) :: z = 0.0_pReal + real(pReal), private :: w = 0.0_pReal + real(pReal), private :: x = 0.0_pReal + real(pReal), private :: y = 0.0_pReal + real(pReal), private :: z = 0.0_pReal contains @@ -82,6 +82,9 @@ module quaternions procedure, public :: log__ procedure, public :: homomorphed => quat_homomorphed + procedure, public :: asArray + procedure, public :: real => real__ + procedure, public :: aimag => aimag__ end type @@ -135,7 +138,7 @@ end function init__ !--------------------------------------------------------------------------------------------------- !> assing a quaternion !--------------------------------------------------------------------------------------------------- -elemental subroutine assign_quat__(self,other) +elemental pure subroutine assign_quat__(self,other) type(quaternion), intent(out) :: self type(quaternion), intent(in) :: other @@ -167,7 +170,7 @@ end subroutine assign_vec__ !--------------------------------------------------------------------------------------------------- !> addition of two quaternions !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function add__(self,other) +type(quaternion) elemental pure function add__(self,other) class(quaternion), intent(in) :: self,other @@ -182,7 +185,7 @@ end function add__ !--------------------------------------------------------------------------------------------------- !> unary positive operator !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function pos__(self) +type(quaternion) elemental pure function pos__(self) class(quaternion), intent(in) :: self @@ -197,7 +200,7 @@ end function pos__ !--------------------------------------------------------------------------------------------------- !> subtraction of two quaternions !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function sub__(self,other) +type(quaternion) elemental pure function sub__(self,other) class(quaternion), intent(in) :: self,other @@ -212,7 +215,7 @@ end function sub__ !--------------------------------------------------------------------------------------------------- !> unary positive operator !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function neg__(self) +type(quaternion) elemental pure function neg__(self) class(quaternion), intent(in) :: self @@ -227,7 +230,7 @@ end function neg__ !--------------------------------------------------------------------------------------------------- !> multiplication of two quaternions !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function mul_quat__(self,other) +type(quaternion) elemental pure function mul_quat__(self,other) class(quaternion), intent(in) :: self, other @@ -242,7 +245,7 @@ end function mul_quat__ !--------------------------------------------------------------------------------------------------- !> multiplication of quaternions with scalar !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function mul_scal__(self,scal) +type(quaternion) elemental pure function mul_scal__(self,scal) class(quaternion), intent(in) :: self real(pReal), intent(in) :: scal @@ -258,7 +261,7 @@ end function mul_scal__ !--------------------------------------------------------------------------------------------------- !> division of two quaternions !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function div_quat__(self,other) +type(quaternion) elemental pure function div_quat__(self,other) class(quaternion), intent(in) :: self, other @@ -270,7 +273,7 @@ end function div_quat__ !--------------------------------------------------------------------------------------------------- !> divisiont of quaternions by scalar !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function div_scal__(self,scal) +type(quaternion) elemental pure function div_scal__(self,scal) class(quaternion), intent(in) :: self real(pReal), intent(in) :: scal @@ -283,7 +286,7 @@ end function div_scal__ !--------------------------------------------------------------------------------------------------- !> equality of two quaternions !--------------------------------------------------------------------------------------------------- -logical elemental function eq__(self,other) +logical elemental pure function eq__(self,other) class(quaternion), intent(in) :: self,other @@ -296,7 +299,7 @@ end function eq__ !--------------------------------------------------------------------------------------------------- !> inequality of two quaternions !--------------------------------------------------------------------------------------------------- -logical elemental function neq__(self,other) +logical elemental pure function neq__(self,other) class(quaternion), intent(in) :: self,other @@ -308,7 +311,7 @@ end function neq__ !--------------------------------------------------------------------------------------------------- !> quaternion to the power of a scalar !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function pow_scal__(self,expon) +type(quaternion) elemental pure function pow_scal__(self,expon) class(quaternion), intent(in) :: self real(pReal), intent(in) :: expon @@ -321,7 +324,7 @@ end function pow_scal__ !--------------------------------------------------------------------------------------------------- !> quaternion to the power of a quaternion !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function pow_quat__(self,expon) +type(quaternion) elemental pure function pow_quat__(self,expon) class(quaternion), intent(in) :: self type(quaternion), intent(in) :: expon @@ -335,7 +338,7 @@ end function pow_quat__ !> exponential of a quaternion !> ToDo: Lacks any check for invalid operations !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function exp__(self) +type(quaternion) elemental pure function exp__(self) class(quaternion), intent(in) :: self real(pReal) :: absImag @@ -354,7 +357,7 @@ end function exp__ !> logarithm of a quaternion !> ToDo: Lacks any check for invalid operations !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function log__(self) +type(quaternion) elemental pure function log__(self) class(quaternion), intent(in) :: self real(pReal) :: absImag @@ -372,7 +375,7 @@ end function log__ !--------------------------------------------------------------------------------------------------- !> norm of a quaternion !--------------------------------------------------------------------------------------------------- -real(pReal) elemental function abs__(a) +real(pReal) elemental pure function abs__(a) class(quaternion), intent(in) :: a @@ -384,7 +387,7 @@ end function abs__ !--------------------------------------------------------------------------------------------------- !> dot product of two quaternions !--------------------------------------------------------------------------------------------------- -real(pReal) elemental function dot_product__(a,b) +real(pReal) elemental pure function dot_product__(a,b) class(quaternion), intent(in) :: a,b @@ -396,7 +399,7 @@ end function dot_product__ !--------------------------------------------------------------------------------------------------- !> conjugate complex of a quaternion !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function conjg__(a) +type(quaternion) elemental pure function conjg__(a) class(quaternion), intent(in) :: a @@ -408,12 +411,52 @@ end function conjg__ !--------------------------------------------------------------------------------------------------- !> homomorphed quaternion of a quaternion !--------------------------------------------------------------------------------------------------- -type(quaternion) elemental function quat_homomorphed(a) +type(quaternion) elemental pure function quat_homomorphed(self) - class(quaternion), intent(in) :: a + class(quaternion), intent(in) :: self - quat_homomorphed = quaternion(-[a%w,a%x,a%y,a%z]) + quat_homomorphed = quaternion(-[self%w,self%x,self%y,self%z]) end function quat_homomorphed + +!--------------------------------------------------------------------------------------------------- +!> quaternion as plain array +!--------------------------------------------------------------------------------------------------- +pure function asArray(self) + + real(pReal), dimension(4) :: asArray + class(quaternion), intent(in) :: self + + asArray = [self%w,self%x,self%y,self%z] + +end function asArray + + +!--------------------------------------------------------------------------------------------------- +!> quaternion as plain array +!--------------------------------------------------------------------------------------------------- +pure function real__(self) + + real(pReal), dimension(3) :: real__ + class(quaternion), intent(in) :: self + + real__ = [self%x,self%y,self%z] + +end function real__ + + +!--------------------------------------------------------------------------------------------------- +!> quaternion as plain array +!--------------------------------------------------------------------------------------------------- +pure function aimag__(self) + + real(pReal) :: aimag__ + class(quaternion), intent(in) :: self + + aimag__ = self%w + +end function aimag__ + + end module quaternions diff --git a/src/rotations.f90 b/src/rotations.f90 index 6604e7b9b..8182328ab 100644 --- a/src/rotations.f90 +++ b/src/rotations.f90 @@ -31,8 +31,8 @@ !> @author Marc De Graef, Carnegie Mellon University !> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH !> @brief rotation storage and conversion -!> @details: rotation is internally stored as quaternion. It cabe inialized from different -!> represantations and also returns itself in different representations. +!> @details: rotation is internally stored as quaternion. It can be inialized from different +!> representations and also returns itself in different representations. ! ! All methods and naming conventions based on Rowenhorst_etal2015 ! Convention 1: coordinate frames are right-handed @@ -87,7 +87,7 @@ pure function asQuaternion(self) class(rotation), intent(in) :: self real(pReal), dimension(4) :: asQuaternion - asQuaternion = [self%q%w, self%q%x, self%q%y, self%q%z] + asQuaternion = self%q%asArray() end function asQuaternion !--------------------------------------------------------------------------------------------------- @@ -96,7 +96,7 @@ pure function asEulerAngles(self) class(rotation), intent(in) :: self real(pReal), dimension(3) :: asEulerAngles - asEulerAngles = qu2eu(self%q) + asEulerAngles = qu2eu(self%q%asArray()) end function asEulerAngles !--------------------------------------------------------------------------------------------------- @@ -105,7 +105,7 @@ pure function asAxisAnglePair(self) class(rotation), intent(in) :: self real(pReal), dimension(4) :: asAxisAnglePair - asAxisAnglePair = qu2ax(self%q) + asAxisAnglePair = qu2ax(self%q%asArray()) end function asAxisAnglePair !--------------------------------------------------------------------------------------------------- @@ -114,7 +114,7 @@ pure function asRotationMatrix(self) class(rotation), intent(in) :: self real(pReal), dimension(3,3) :: asRotationMatrix - asRotationMatrix = qu2om(self%q) + asRotationMatrix = qu2om(self%q%asArray()) end function asRotationMatrix !--------------------------------------------------------------------------------------------------- @@ -123,7 +123,7 @@ pure function asRodriguesFrankVector(self) class(rotation), intent(in) :: self real(pReal), dimension(4) :: asRodriguesFrankVector - asRodriguesFrankVector = qu2ro(self%q) + asRodriguesFrankVector = qu2ro(self%q%asArray()) end function asRodriguesFrankVector !--------------------------------------------------------------------------------------------------- @@ -132,7 +132,7 @@ pure function asHomochoric(self) class(rotation), intent(in) :: self real(pReal), dimension(3) :: asHomochoric - asHomochoric = qu2ho(self%q) + asHomochoric = qu2ho(self%q%asArray()) end function asHomochoric @@ -230,8 +230,8 @@ function rotVector(self,v,active) result(vRot) real(pReal), intent(in), dimension(3) :: v logical, intent(in), optional :: active - type(quaternion) :: q - logical :: passive + type(quaternion) :: q + logical :: passive if (present(active)) then passive = .not. active @@ -245,7 +245,7 @@ function rotVector(self,v,active) result(vRot) else q = conjg(self%q) * (quaternion([0.0_pReal, v(1), v(2), v(3)]) * self%q ) endif - vRot = [q%x,q%y,q%z] + vRot = q%real() else if (passive) then vRot = matmul(self%asRotationMatrix(),v) @@ -305,24 +305,24 @@ end function misorientation !--------------------------------------------------------------------------------------------------- pure function qu2om(qu) result(om) - type(quaternion), intent(in) :: qu - real(pReal), dimension(3,3) :: om + real(pReal), intent(in), dimension(4) :: qu + real(pReal), dimension(3,3) :: om - real(pReal) :: qq + real(pReal) :: qq - qq = qu%w**2-(qu%x**2 + qu%y**2 + qu%z**2) + qq = qu(1)**2-sum(qu(2:4)**2) - om(1,1) = qq+2.0*qu%x*qu%x - om(2,2) = qq+2.0*qu%y*qu%y - om(3,3) = qq+2.0*qu%z*qu%z + om(1,1) = qq+2.0*qu(2)**2 + om(2,2) = qq+2.0*qu(3)**2 + om(3,3) = qq+2.0*qu(4)**2 - om(1,2) = 2.0*(qu%x*qu%y-qu%w*qu%z) - om(2,3) = 2.0*(qu%y*qu%z-qu%w*qu%x) - om(3,1) = 2.0*(qu%z*qu%x-qu%w*qu%y) - om(2,1) = 2.0*(qu%y*qu%x+qu%w*qu%z) - om(3,2) = 2.0*(qu%z*qu%y+qu%w*qu%x) - om(1,3) = 2.0*(qu%x*qu%z+qu%w*qu%y) + om(1,2) = 2.0*(qu(2)*qu(3)-qu(1)*qu(4)) + om(2,3) = 2.0*(qu(3)*qu(4)-qu(1)*qu(2)) + om(3,1) = 2.0*(qu(4)*qu(2)-qu(1)*qu(3)) + om(2,1) = 2.0*(qu(3)*qu(2)+qu(1)*qu(4)) + om(3,2) = 2.0*(qu(4)*qu(3)+qu(1)*qu(2)) + om(1,3) = 2.0*(qu(2)*qu(4)+qu(1)*qu(3)) if (P < 0.0) om = transpose(om) @@ -335,24 +335,24 @@ end function qu2om !--------------------------------------------------------------------------------------------------- pure function qu2eu(qu) result(eu) - type(quaternion), intent(in) :: qu - real(pReal), dimension(3) :: eu + real(pReal), intent(in), dimension(4) :: qu + real(pReal), dimension(3) :: eu - real(pReal) :: q12, q03, chi, chiInv + real(pReal) :: q12, q03, chi, chiInv - q03 = qu%w**2+qu%z**2 - q12 = qu%x**2+qu%y**2 + q03 = qu(1)**2+qu(4)**2 + q12 = qu(2)**2+qu(3)**2 chi = sqrt(q03*q12) degenerated: if (dEq0(chi)) then - eu = merge([atan2(-P*2.0*qu%w*qu%z,qu%w**2-qu%z**2), 0.0_pReal, 0.0_pReal], & - [atan2(2.0*qu%x*qu%y,qu%x**2-qu%y**2), PI, 0.0_pReal], & + eu = merge([atan2(-P*2.0*qu(1)*qu(4),qu(1)**2-qu(4)**2), 0.0_pReal, 0.0_pReal], & + [atan2( 2.0*qu(2)*qu(3),qu(2)**2-qu(3)**2), PI, 0.0_pReal], & dEq0(q12)) else degenerated chiInv = 1.0/chi - eu = [atan2((-P*qu%w*qu%y+qu%x*qu%z)*chi, (-P*qu%w*qu%x-qu%y*qu%z)*chi ), & + eu = [atan2((-P*qu(1)*qu(3)+qu(2)*qu(4))*chi, (-P*qu(1)*qu(2)-qu(3)*qu(4))*chi ), & atan2( 2.0*chi, q03-q12 ), & - atan2(( P*qu%w*qu%y+qu%x*qu%z)*chi, (-P*qu%w*qu%x+qu%y*qu%z)*chi )] + atan2(( P*qu(1)*qu(3)+qu(2)*qu(4))*chi, (-P*qu(1)*qu(2)+qu(3)*qu(4))*chi )] endif degenerated where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI]) @@ -365,19 +365,19 @@ end function qu2eu !--------------------------------------------------------------------------------------------------- pure function qu2ax(qu) result(ax) - type(quaternion), intent(in) :: qu - real(pReal), dimension(4) :: ax + real(pReal), intent(in), dimension(4) :: qu + real(pReal), dimension(4) :: ax - real(pReal) :: omega, s + real(pReal) :: omega, s - if (dEq0(qu%x**2+qu%y**2+qu%z**2)) then + if (dEq0(sum(qu(2:4)**2))) then ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ] ! axis = [001] - elseif (dNeq0(qu%w)) then - s = sign(1.0_pReal,qu%w)/sqrt(qu%x**2+qu%y**2+qu%z**2) - omega = 2.0_pReal * acos(math_clip(qu%w,-1.0_pReal,1.0_pReal)) - ax = [ qu%x*s, qu%y*s, qu%z*s, omega ] + elseif (dNeq0(qu(1))) then + s = sign(1.0_pReal,qu(1))/norm2(qu(2:4)) + omega = 2.0_pReal * acos(math_clip(qu(1),-1.0_pReal,1.0_pReal)) + ax = [ qu(2)*s, qu(3)*s, qu(4)*s, omega ] else - ax = [ qu%x, qu%y, qu%z, PI ] + ax = [ qu(2), qu(3), qu(4), PI ] end if end function qu2ax @@ -389,20 +389,20 @@ end function qu2ax !--------------------------------------------------------------------------------------------------- pure function qu2ro(qu) result(ro) - type(quaternion), intent(in) :: qu - real(pReal), dimension(4) :: ro + real(pReal), intent(in), dimension(4) :: qu + real(pReal), dimension(4) :: ro - real(pReal) :: s - real(pReal), parameter :: thr = 1.0e-8_pReal + real(pReal) :: s + real(pReal), parameter :: thr = 1.0e-8_pReal - if (qu%w < thr) then - ro = [qu%x, qu%y, qu%z, IEEE_value(ro(4),IEEE_positive_inf)] + if (qu(1) < thr) then + ro = [qu(2), qu(3), qu(4), IEEE_value(ro(4),IEEE_positive_inf)] else - s = norm2([qu%x,qu%y,qu%z]) + s = norm2(qu(2:4)) if (s < thr) then - ro = [ 0.0_pReal, 0.0_pReal, P, 0.0_pReal] + ro = [0.0_pReal, 0.0_pReal, P, 0.0_pReal] else - ro = [ qu%x/s, qu%y/s, qu%z/s, tan(acos(math_clip(qu%w,-1.0_pReal,1.0_pReal)))] + ro = [qu(2)/s,qu(3)/s,qu(4)/s, tan(acos(math_clip(qu(1),-1.0_pReal,1.0_pReal)))] endif end if @@ -416,17 +416,17 @@ end function qu2ro !--------------------------------------------------------------------------------------------------- pure function qu2ho(qu) result(ho) - type(quaternion), intent(in) :: qu - real(pReal), dimension(3) :: ho + real(pReal), intent(in), dimension(4) :: qu + real(pReal), dimension(3) :: ho - real(pReal) :: omega, f + real(pReal) :: omega, f - omega = 2.0 * acos(math_clip(qu%w,-1.0_pReal,1.0_pReal)) + omega = 2.0 * acos(math_clip(qu(1),-1.0_pReal,1.0_pReal)) if (dEq0(omega)) then ho = [ 0.0, 0.0, 0.0 ] else - ho = [qu%x, qu%y, qu%z] + ho = qu(2:4) f = 0.75 * ( omega - sin(omega) ) ho = ho/norm2(ho)* f**(1.0/3.0) end if @@ -440,8 +440,8 @@ end function qu2ho !--------------------------------------------------------------------------------------------------- function qu2cu(qu) result(cu) - type(quaternion), intent(in) :: qu - real(pReal), dimension(3) :: cu + real(pReal), intent(in), dimension(4) :: qu + real(pReal), dimension(3) :: cu cu = ho2cu(qu2ho(qu)) @@ -456,7 +456,7 @@ end function qu2cu pure function om2qu(om) result(qu) real(pReal), intent(in), dimension(3,3) :: om - type(quaternion) :: qu + real(pReal), dimension(4) :: qu qu = eu2qu(om2eu(om)) @@ -578,21 +578,21 @@ end function om2cu !--------------------------------------------------------------------------------------------------- pure function eu2qu(eu) result(qu) - real(pReal), intent(in), dimension(3) :: eu - type(quaternion) :: qu - real(pReal), dimension(3) :: ee - real(pReal) :: cPhi, sPhi + real(pReal), intent(in), dimension(3) :: eu + real(pReal), dimension(4) :: qu + real(pReal), dimension(3) :: ee + real(pReal) :: cPhi, sPhi ee = 0.5_pReal*eu cPhi = cos(ee(2)) sPhi = sin(ee(2)) - qu = quaternion([ cPhi*cos(ee(1)+ee(3)), & - -P*sPhi*cos(ee(1)-ee(3)), & - -P*sPhi*sin(ee(1)-ee(3)), & - -P*cPhi*sin(ee(1)+ee(3))]) - if(qu%w < 0.0_pReal) qu = qu%homomorphed() + qu = [ cPhi*cos(ee(1)+ee(3)), & + -P*sPhi*cos(ee(1)-ee(3)), & + -P*sPhi*sin(ee(1)-ee(3)), & + -P*cPhi*sin(ee(1)+ee(3))] + if(qu(1) < 0.0_pReal) qu = qu * (-1.0_pReal) end function eu2qu @@ -710,18 +710,18 @@ end function eu2cu !--------------------------------------------------------------------------------------------------- pure function ax2qu(ax) result(qu) - real(pReal), intent(in), dimension(4) :: ax - type(quaternion) :: qu - - real(pReal) :: c, s + real(pReal), intent(in), dimension(4) :: ax + real(pReal), dimension(4) :: qu + + real(pReal) :: c, s if (dEq0(ax(4))) then - qu = quaternion([ 1.0_pReal, 0.0_pReal, 0.0_pReal, 0.0_pReal ]) + qu = [ 1.0_pReal, 0.0_pReal, 0.0_pReal, 0.0_pReal ] else c = cos(ax(4)*0.5) s = sin(ax(4)*0.5) - qu = quaternion([ c, ax(1)*s, ax(2)*s, ax(3)*s ]) + qu = [ c, ax(1)*s, ax(2)*s, ax(3)*s ] end if end function ax2qu @@ -837,8 +837,8 @@ end function ax2cu !--------------------------------------------------------------------------------------------------- pure function ro2qu(ro) result(qu) - real(pReal), intent(in), dimension(4) :: ro - type(quaternion) :: qu + real(pReal), intent(in), dimension(4) :: ro + real(pReal), dimension(4) :: qu qu = ax2qu(ro2ax(ro)) @@ -940,8 +940,8 @@ end function ro2cu !--------------------------------------------------------------------------------------------------- pure function ho2qu(ho) result(qu) - real(pReal), intent(in), dimension(3) :: ho - type(quaternion) :: qu + real(pReal), intent(in), dimension(3) :: ho + real(pReal), dimension(4) :: qu qu = ax2qu(ho2ax(ho)) @@ -1051,7 +1051,7 @@ end function ho2cu function cu2qu(cu) result(qu) real(pReal), intent(in), dimension(3) :: cu - type(quaternion) :: qu + real(pReal), dimension(4) :: qu qu = ho2qu(cu2ho(cu))