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
This commit is contained in:
Martin Diehl 2019-09-20 08:36:16 -07:00
parent 0b6620bfb7
commit d69d57221d
2 changed files with 147 additions and 104 deletions

View File

@ -42,10 +42,10 @@ module quaternions
real(pReal), parameter, public :: P = -1.0_pReal !< parameter for orientation conversion. real(pReal), parameter, public :: P = -1.0_pReal !< parameter for orientation conversion.
type, public :: quaternion type, public :: quaternion
real(pReal) :: w = 0.0_pReal real(pReal), private :: w = 0.0_pReal
real(pReal) :: x = 0.0_pReal real(pReal), private :: x = 0.0_pReal
real(pReal) :: y = 0.0_pReal real(pReal), private :: y = 0.0_pReal
real(pReal) :: z = 0.0_pReal real(pReal), private :: z = 0.0_pReal
contains contains
@ -82,6 +82,9 @@ module quaternions
procedure, public :: log__ procedure, public :: log__
procedure, public :: homomorphed => quat_homomorphed procedure, public :: homomorphed => quat_homomorphed
procedure, public :: asArray
procedure, public :: real => real__
procedure, public :: aimag => aimag__
end type end type
@ -135,7 +138,7 @@ end function init__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> assing a quaternion !> assing a quaternion
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
elemental subroutine assign_quat__(self,other) elemental pure subroutine assign_quat__(self,other)
type(quaternion), intent(out) :: self type(quaternion), intent(out) :: self
type(quaternion), intent(in) :: other type(quaternion), intent(in) :: other
@ -167,7 +170,7 @@ end subroutine assign_vec__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> addition of two quaternions !> 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 class(quaternion), intent(in) :: self,other
@ -182,7 +185,7 @@ end function add__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> unary positive operator !> unary positive operator
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
type(quaternion) elemental function pos__(self) type(quaternion) elemental pure function pos__(self)
class(quaternion), intent(in) :: self class(quaternion), intent(in) :: self
@ -197,7 +200,7 @@ end function pos__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> subtraction of two quaternions !> 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 class(quaternion), intent(in) :: self,other
@ -212,7 +215,7 @@ end function sub__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> unary positive operator !> unary positive operator
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
type(quaternion) elemental function neg__(self) type(quaternion) elemental pure function neg__(self)
class(quaternion), intent(in) :: self class(quaternion), intent(in) :: self
@ -227,7 +230,7 @@ end function neg__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> multiplication of two quaternions !> 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 class(quaternion), intent(in) :: self, other
@ -242,7 +245,7 @@ end function mul_quat__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> multiplication of quaternions with scalar !> 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 class(quaternion), intent(in) :: self
real(pReal), intent(in) :: scal real(pReal), intent(in) :: scal
@ -258,7 +261,7 @@ end function mul_scal__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> division of two quaternions !> 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 class(quaternion), intent(in) :: self, other
@ -270,7 +273,7 @@ end function div_quat__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> divisiont of quaternions by scalar !> 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 class(quaternion), intent(in) :: self
real(pReal), intent(in) :: scal real(pReal), intent(in) :: scal
@ -283,7 +286,7 @@ end function div_scal__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> equality of two quaternions !> equality of two quaternions
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
logical elemental function eq__(self,other) logical elemental pure function eq__(self,other)
class(quaternion), intent(in) :: self,other class(quaternion), intent(in) :: self,other
@ -296,7 +299,7 @@ end function eq__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> inequality of two quaternions !> inequality of two quaternions
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
logical elemental function neq__(self,other) logical elemental pure function neq__(self,other)
class(quaternion), intent(in) :: self,other class(quaternion), intent(in) :: self,other
@ -308,7 +311,7 @@ end function neq__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> quaternion to the power of a scalar !> 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 class(quaternion), intent(in) :: self
real(pReal), intent(in) :: expon real(pReal), intent(in) :: expon
@ -321,7 +324,7 @@ end function pow_scal__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> quaternion to the power of a quaternion !> 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 class(quaternion), intent(in) :: self
type(quaternion), intent(in) :: expon type(quaternion), intent(in) :: expon
@ -335,7 +338,7 @@ end function pow_quat__
!> exponential of a quaternion !> exponential of a quaternion
!> ToDo: Lacks any check for invalid operations !> 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 class(quaternion), intent(in) :: self
real(pReal) :: absImag real(pReal) :: absImag
@ -354,7 +357,7 @@ end function exp__
!> logarithm of a quaternion !> logarithm of a quaternion
!> ToDo: Lacks any check for invalid operations !> 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 class(quaternion), intent(in) :: self
real(pReal) :: absImag real(pReal) :: absImag
@ -372,7 +375,7 @@ end function log__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> norm of a quaternion !> norm of a quaternion
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
real(pReal) elemental function abs__(a) real(pReal) elemental pure function abs__(a)
class(quaternion), intent(in) :: a class(quaternion), intent(in) :: a
@ -384,7 +387,7 @@ end function abs__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> dot product of two quaternions !> 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 class(quaternion), intent(in) :: a,b
@ -396,7 +399,7 @@ end function dot_product__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> conjugate complex of a quaternion !> conjugate complex of a quaternion
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
type(quaternion) elemental function conjg__(a) type(quaternion) elemental pure function conjg__(a)
class(quaternion), intent(in) :: a class(quaternion), intent(in) :: a
@ -408,12 +411,52 @@ end function conjg__
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
!> homomorphed quaternion of a quaternion !> 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 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 end module quaternions

View File

@ -31,8 +31,8 @@
!> @author Marc De Graef, Carnegie Mellon University !> @author Marc De Graef, Carnegie Mellon University
!> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH !> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH
!> @brief rotation storage and conversion !> @brief rotation storage and conversion
!> @details: rotation is internally stored as quaternion. It cabe inialized from different !> @details: rotation is internally stored as quaternion. It can be inialized from different
!> represantations and also returns itself in different representations. !> representations and also returns itself in different representations.
! !
! All methods and naming conventions based on Rowenhorst_etal2015 ! All methods and naming conventions based on Rowenhorst_etal2015
! Convention 1: coordinate frames are right-handed ! Convention 1: coordinate frames are right-handed
@ -87,7 +87,7 @@ pure function asQuaternion(self)
class(rotation), intent(in) :: self class(rotation), intent(in) :: self
real(pReal), dimension(4) :: asQuaternion 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 end function asQuaternion
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
@ -96,7 +96,7 @@ pure function asEulerAngles(self)
class(rotation), intent(in) :: self class(rotation), intent(in) :: self
real(pReal), dimension(3) :: asEulerAngles real(pReal), dimension(3) :: asEulerAngles
asEulerAngles = qu2eu(self%q) asEulerAngles = qu2eu(self%q%asArray())
end function asEulerAngles end function asEulerAngles
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
@ -105,7 +105,7 @@ pure function asAxisAnglePair(self)
class(rotation), intent(in) :: self class(rotation), intent(in) :: self
real(pReal), dimension(4) :: asAxisAnglePair real(pReal), dimension(4) :: asAxisAnglePair
asAxisAnglePair = qu2ax(self%q) asAxisAnglePair = qu2ax(self%q%asArray())
end function asAxisAnglePair end function asAxisAnglePair
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
@ -114,7 +114,7 @@ pure function asRotationMatrix(self)
class(rotation), intent(in) :: self class(rotation), intent(in) :: self
real(pReal), dimension(3,3) :: asRotationMatrix real(pReal), dimension(3,3) :: asRotationMatrix
asRotationMatrix = qu2om(self%q) asRotationMatrix = qu2om(self%q%asArray())
end function asRotationMatrix end function asRotationMatrix
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
@ -123,7 +123,7 @@ pure function asRodriguesFrankVector(self)
class(rotation), intent(in) :: self class(rotation), intent(in) :: self
real(pReal), dimension(4) :: asRodriguesFrankVector real(pReal), dimension(4) :: asRodriguesFrankVector
asRodriguesFrankVector = qu2ro(self%q) asRodriguesFrankVector = qu2ro(self%q%asArray())
end function asRodriguesFrankVector end function asRodriguesFrankVector
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
@ -132,7 +132,7 @@ pure function asHomochoric(self)
class(rotation), intent(in) :: self class(rotation), intent(in) :: self
real(pReal), dimension(3) :: asHomochoric real(pReal), dimension(3) :: asHomochoric
asHomochoric = qu2ho(self%q) asHomochoric = qu2ho(self%q%asArray())
end function asHomochoric end function asHomochoric
@ -245,7 +245,7 @@ function rotVector(self,v,active) result(vRot)
else else
q = conjg(self%q) * (quaternion([0.0_pReal, v(1), v(2), v(3)]) * self%q ) q = conjg(self%q) * (quaternion([0.0_pReal, v(1), v(2), v(3)]) * self%q )
endif endif
vRot = [q%x,q%y,q%z] vRot = q%real()
else else
if (passive) then if (passive) then
vRot = matmul(self%asRotationMatrix(),v) vRot = matmul(self%asRotationMatrix(),v)
@ -305,24 +305,24 @@ end function misorientation
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
pure function qu2om(qu) result(om) pure function qu2om(qu) result(om)
type(quaternion), intent(in) :: qu real(pReal), intent(in), dimension(4) :: qu
real(pReal), dimension(3,3) :: om 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(1,1) = qq+2.0*qu(2)**2
om(2,2) = qq+2.0*qu%y*qu%y om(2,2) = qq+2.0*qu(3)**2
om(3,3) = qq+2.0*qu%z*qu%z om(3,3) = qq+2.0*qu(4)**2
om(1,2) = 2.0*(qu%x*qu%y-qu%w*qu%z) om(1,2) = 2.0*(qu(2)*qu(3)-qu(1)*qu(4))
om(2,3) = 2.0*(qu%y*qu%z-qu%w*qu%x) om(2,3) = 2.0*(qu(3)*qu(4)-qu(1)*qu(2))
om(3,1) = 2.0*(qu%z*qu%x-qu%w*qu%y) om(3,1) = 2.0*(qu(4)*qu(2)-qu(1)*qu(3))
om(2,1) = 2.0*(qu%y*qu%x+qu%w*qu%z) om(2,1) = 2.0*(qu(3)*qu(2)+qu(1)*qu(4))
om(3,2) = 2.0*(qu%z*qu%y+qu%w*qu%x) om(3,2) = 2.0*(qu(4)*qu(3)+qu(1)*qu(2))
om(1,3) = 2.0*(qu%x*qu%z+qu%w*qu%y) om(1,3) = 2.0*(qu(2)*qu(4)+qu(1)*qu(3))
if (P < 0.0) om = transpose(om) if (P < 0.0) om = transpose(om)
@ -335,24 +335,24 @@ end function qu2om
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
pure function qu2eu(qu) result(eu) pure function qu2eu(qu) result(eu)
type(quaternion), intent(in) :: qu real(pReal), intent(in), dimension(4) :: qu
real(pReal), dimension(3) :: eu real(pReal), dimension(3) :: eu
real(pReal) :: q12, q03, chi, chiInv real(pReal) :: q12, q03, chi, chiInv
q03 = qu%w**2+qu%z**2 q03 = qu(1)**2+qu(4)**2
q12 = qu%x**2+qu%y**2 q12 = qu(2)**2+qu(3)**2
chi = sqrt(q03*q12) chi = sqrt(q03*q12)
degenerated: if (dEq0(chi)) then 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], & 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%x*qu%y,qu%x**2-qu%y**2), PI, 0.0_pReal], & [atan2( 2.0*qu(2)*qu(3),qu(2)**2-qu(3)**2), PI, 0.0_pReal], &
dEq0(q12)) dEq0(q12))
else degenerated else degenerated
chiInv = 1.0/chi 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( 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 endif degenerated
where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI]) 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) pure function qu2ax(qu) result(ax)
type(quaternion), intent(in) :: qu real(pReal), intent(in), dimension(4) :: qu
real(pReal), dimension(4) :: ax 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] ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ] ! axis = [001]
elseif (dNeq0(qu%w)) then elseif (dNeq0(qu(1))) then
s = sign(1.0_pReal,qu%w)/sqrt(qu%x**2+qu%y**2+qu%z**2) s = sign(1.0_pReal,qu(1))/norm2(qu(2:4))
omega = 2.0_pReal * acos(math_clip(qu%w,-1.0_pReal,1.0_pReal)) omega = 2.0_pReal * acos(math_clip(qu(1),-1.0_pReal,1.0_pReal))
ax = [ qu%x*s, qu%y*s, qu%z*s, omega ] ax = [ qu(2)*s, qu(3)*s, qu(4)*s, omega ]
else else
ax = [ qu%x, qu%y, qu%z, PI ] ax = [ qu(2), qu(3), qu(4), PI ]
end if end if
end function qu2ax end function qu2ax
@ -389,20 +389,20 @@ end function qu2ax
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
pure function qu2ro(qu) result(ro) pure function qu2ro(qu) result(ro)
type(quaternion), intent(in) :: qu real(pReal), intent(in), dimension(4) :: qu
real(pReal), dimension(4) :: ro real(pReal), dimension(4) :: ro
real(pReal) :: s real(pReal) :: s
real(pReal), parameter :: thr = 1.0e-8_pReal real(pReal), parameter :: thr = 1.0e-8_pReal
if (qu%w < thr) then if (qu(1) < thr) then
ro = [qu%x, qu%y, qu%z, IEEE_value(ro(4),IEEE_positive_inf)] ro = [qu(2), qu(3), qu(4), IEEE_value(ro(4),IEEE_positive_inf)]
else else
s = norm2([qu%x,qu%y,qu%z]) s = norm2(qu(2:4))
if (s < thr) then 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 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 endif
end if end if
@ -416,17 +416,17 @@ end function qu2ro
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
pure function qu2ho(qu) result(ho) pure function qu2ho(qu) result(ho)
type(quaternion), intent(in) :: qu real(pReal), intent(in), dimension(4) :: qu
real(pReal), dimension(3) :: ho 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 if (dEq0(omega)) then
ho = [ 0.0, 0.0, 0.0 ] ho = [ 0.0, 0.0, 0.0 ]
else else
ho = [qu%x, qu%y, qu%z] ho = qu(2:4)
f = 0.75 * ( omega - sin(omega) ) f = 0.75 * ( omega - sin(omega) )
ho = ho/norm2(ho)* f**(1.0/3.0) ho = ho/norm2(ho)* f**(1.0/3.0)
end if end if
@ -440,7 +440,7 @@ end function qu2ho
!--------------------------------------------------------------------------------------------------- !---------------------------------------------------------------------------------------------------
function qu2cu(qu) result(cu) function qu2cu(qu) result(cu)
type(quaternion), intent(in) :: qu real(pReal), intent(in), dimension(4) :: qu
real(pReal), dimension(3) :: cu real(pReal), dimension(3) :: cu
cu = ho2cu(qu2ho(qu)) cu = ho2cu(qu2ho(qu))
@ -456,7 +456,7 @@ end function qu2cu
pure function om2qu(om) result(qu) pure function om2qu(om) result(qu)
real(pReal), intent(in), dimension(3,3) :: om real(pReal), intent(in), dimension(3,3) :: om
type(quaternion) :: qu real(pReal), dimension(4) :: qu
qu = eu2qu(om2eu(om)) qu = eu2qu(om2eu(om))
@ -579,7 +579,7 @@ end function om2cu
pure function eu2qu(eu) result(qu) pure function eu2qu(eu) result(qu)
real(pReal), intent(in), dimension(3) :: eu real(pReal), intent(in), dimension(3) :: eu
type(quaternion) :: qu real(pReal), dimension(4) :: qu
real(pReal), dimension(3) :: ee real(pReal), dimension(3) :: ee
real(pReal) :: cPhi, sPhi real(pReal) :: cPhi, sPhi
@ -588,11 +588,11 @@ pure function eu2qu(eu) result(qu)
cPhi = cos(ee(2)) cPhi = cos(ee(2))
sPhi = sin(ee(2)) sPhi = sin(ee(2))
qu = quaternion([ cPhi*cos(ee(1)+ee(3)), & qu = [ cPhi*cos(ee(1)+ee(3)), &
-P*sPhi*cos(ee(1)-ee(3)), & -P*sPhi*cos(ee(1)-ee(3)), &
-P*sPhi*sin(ee(1)-ee(3)), & -P*sPhi*sin(ee(1)-ee(3)), &
-P*cPhi*sin(ee(1)+ee(3))]) -P*cPhi*sin(ee(1)+ee(3))]
if(qu%w < 0.0_pReal) qu = qu%homomorphed() if(qu(1) < 0.0_pReal) qu = qu * (-1.0_pReal)
end function eu2qu end function eu2qu
@ -711,17 +711,17 @@ end function eu2cu
pure function ax2qu(ax) result(qu) pure function ax2qu(ax) result(qu)
real(pReal), intent(in), dimension(4) :: ax real(pReal), intent(in), dimension(4) :: ax
type(quaternion) :: qu real(pReal), dimension(4) :: qu
real(pReal) :: c, s real(pReal) :: c, s
if (dEq0(ax(4))) then 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 else
c = cos(ax(4)*0.5) c = cos(ax(4)*0.5)
s = sin(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 if
end function ax2qu end function ax2qu
@ -838,7 +838,7 @@ end function ax2cu
pure function ro2qu(ro) result(qu) pure function ro2qu(ro) result(qu)
real(pReal), intent(in), dimension(4) :: ro real(pReal), intent(in), dimension(4) :: ro
type(quaternion) :: qu real(pReal), dimension(4) :: qu
qu = ax2qu(ro2ax(ro)) qu = ax2qu(ro2ax(ro))
@ -941,7 +941,7 @@ end function ro2cu
pure function ho2qu(ho) result(qu) pure function ho2qu(ho) result(qu)
real(pReal), intent(in), dimension(3) :: ho real(pReal), intent(in), dimension(3) :: ho
type(quaternion) :: qu real(pReal), dimension(4) :: qu
qu = ax2qu(ho2ax(ho)) qu = ax2qu(ho2ax(ho))
@ -1051,7 +1051,7 @@ end function ho2cu
function cu2qu(cu) result(qu) function cu2qu(cu) result(qu)
real(pReal), intent(in), dimension(3) :: cu real(pReal), intent(in), dimension(3) :: cu
type(quaternion) :: qu real(pReal), dimension(4) :: qu
qu = ho2qu(cu2ho(cu)) qu = ho2qu(cu2ho(cu))