same name as in the python module
This commit is contained in:
parent
407f94082f
commit
feb87c7db8
|
@ -40,7 +40,7 @@ module quaternions
|
||||||
implicit none
|
implicit none
|
||||||
public
|
public
|
||||||
|
|
||||||
real(pReal), parameter, public :: epsijk = -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) :: w = 0.0_pReal
|
||||||
|
@ -241,9 +241,9 @@ type(quaternion) elemental function mul_quat__(self,other)
|
||||||
class(quaternion), intent(in) :: self, other
|
class(quaternion), intent(in) :: self, other
|
||||||
|
|
||||||
mul_quat__%w = self%w*other%w - self%x*other%x - self%y*other%y - self%z*other%z
|
mul_quat__%w = self%w*other%w - self%x*other%x - self%y*other%y - self%z*other%z
|
||||||
mul_quat__%x = self%w*other%x + self%x*other%w + epsijk * (self%y*other%z - self%z*other%y)
|
mul_quat__%x = self%w*other%x + self%x*other%w + P * (self%y*other%z - self%z*other%y)
|
||||||
mul_quat__%y = self%w*other%y + self%y*other%w + epsijk * (self%z*other%x - self%x*other%z)
|
mul_quat__%y = self%w*other%y + self%y*other%w + P * (self%z*other%x - self%x*other%z)
|
||||||
mul_quat__%z = self%w*other%z + self%z*other%w + epsijk * (self%x*other%y - self%y*other%x)
|
mul_quat__%z = self%w*other%z + self%z*other%w + P * (self%x*other%y - self%y*other%x)
|
||||||
|
|
||||||
end function mul_quat__
|
end function mul_quat__
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,7 @@
|
||||||
! Convention 4: Euler angle triplets are implemented using the Bunge convention,
|
! Convention 4: Euler angle triplets are implemented using the Bunge convention,
|
||||||
! with the angular ranges as [0, 2π],[0, π],[0, 2π]
|
! with the angular ranges as [0, 2π],[0, π],[0, 2π]
|
||||||
! Convention 5: the rotation angle ω is limited to the interval [0, π]
|
! Convention 5: the rotation angle ω is limited to the interval [0, π]
|
||||||
! Convention 6: epsijk/P = -1
|
! Convention 6: P = -1
|
||||||
!---------------------------------------------------------------------------------------------------
|
!---------------------------------------------------------------------------------------------------
|
||||||
|
|
||||||
module rotations
|
module rotations
|
||||||
|
@ -280,7 +280,7 @@ pure function eu2ax(eu) result(ax)
|
||||||
if (dEq0(alpha)) then ! return a default identity axis-angle pair
|
if (dEq0(alpha)) then ! return a default identity axis-angle pair
|
||||||
ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ]
|
ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ]
|
||||||
else
|
else
|
||||||
ax(1:3) = -epsijk/tau * [ t*cos(delta), t*sin(delta), sin(sigma) ] ! passive axis-angle pair so a minus sign in front
|
ax(1:3) = -P/tau * [ t*cos(delta), t*sin(delta), sin(sigma) ] ! passive axis-angle pair so a minus sign in front
|
||||||
ax(4) = alpha
|
ax(4) = alpha
|
||||||
if (alpha < 0.0) ax = -ax ! ensure alpha is positive
|
if (alpha < 0.0) ax = -ax ! ensure alpha is positive
|
||||||
end if
|
end if
|
||||||
|
@ -309,7 +309,7 @@ pure function eu2ro(eu) result(ro)
|
||||||
if (ro(4) >= PI) then
|
if (ro(4) >= PI) then
|
||||||
ro(4) = IEEE_value(ro(4),IEEE_positive_inf)
|
ro(4) = IEEE_value(ro(4),IEEE_positive_inf)
|
||||||
elseif(dEq0(ro(4))) then
|
elseif(dEq0(ro(4))) then
|
||||||
ro = [ 0.0_pReal, 0.0_pReal, epsijk, 0.0_pReal ]
|
ro = [ 0.0_pReal, 0.0_pReal, P, 0.0_pReal ]
|
||||||
else
|
else
|
||||||
ro(4) = tan(ro(4)*0.5)
|
ro(4) = tan(ro(4)*0.5)
|
||||||
end if
|
end if
|
||||||
|
@ -335,9 +335,9 @@ pure function eu2qu(eu) result(qu)
|
||||||
sPhi = sin(ee(2))
|
sPhi = sin(ee(2))
|
||||||
|
|
||||||
qu = quaternion([ cPhi*cos(ee(1)+ee(3)), &
|
qu = quaternion([ cPhi*cos(ee(1)+ee(3)), &
|
||||||
-epsijk*sPhi*cos(ee(1)-ee(3)), &
|
-P*sPhi*cos(ee(1)-ee(3)), &
|
||||||
-epsijk*sPhi*sin(ee(1)-ee(3)), &
|
-P*sPhi*sin(ee(1)-ee(3)), &
|
||||||
-epsijk*cPhi*sin(ee(1)+ee(3))])
|
-P*cPhi*sin(ee(1)+ee(3))])
|
||||||
if(qu%w < 0.0_pReal) qu = qu%homomorphed()
|
if(qu%w < 0.0_pReal) qu = qu%homomorphed()
|
||||||
|
|
||||||
end function eu2qu
|
end function eu2qu
|
||||||
|
@ -404,7 +404,7 @@ pure function ax2om(ax) result(om)
|
||||||
om(3,1) = q + s*ax(2)
|
om(3,1) = q + s*ax(2)
|
||||||
om(1,3) = q - s*ax(2)
|
om(1,3) = q - s*ax(2)
|
||||||
|
|
||||||
if (epsijk > 0.0) om = transpose(om)
|
if (P > 0.0) om = transpose(om)
|
||||||
|
|
||||||
end function ax2om
|
end function ax2om
|
||||||
|
|
||||||
|
@ -430,14 +430,14 @@ pure function qu2eu(qu) result(eu)
|
||||||
chi = sqrt(q03*q12)
|
chi = sqrt(q03*q12)
|
||||||
|
|
||||||
degenerated: if (dEq0(chi)) then
|
degenerated: if (dEq0(chi)) then
|
||||||
eu = merge([atan2(-epsijk*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%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], &
|
[atan2(2.0*qu%x*qu%y,qu%x**2-qu%y**2), PI, 0.0_pReal], &
|
||||||
dEq0(q12))
|
dEq0(q12))
|
||||||
else degenerated
|
else degenerated
|
||||||
chiInv = 1.0/chi
|
chiInv = 1.0/chi
|
||||||
eu = [atan2((-epsijk*qu%w*qu%y+qu%x*qu%z)*chi, (-epsijk*qu%w*qu%x-qu%y*qu%z)*chi ), &
|
eu = [atan2((-P*qu%w*qu%y+qu%x*qu%z)*chi, (-P*qu%w*qu%x-qu%y*qu%z)*chi ), &
|
||||||
atan2( 2.0*chi, q03-q12 ), &
|
atan2( 2.0*chi, q03-q12 ), &
|
||||||
atan2(( epsijk*qu%w*qu%y+qu%x*qu%z)*chi, (-epsijk*qu%w*qu%x+qu%y*qu%z)*chi )]
|
atan2(( P*qu%w*qu%y+qu%x*qu%z)*chi, (-P*qu%w*qu%x+qu%y*qu%z)*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])
|
||||||
|
|
||||||
|
@ -560,7 +560,7 @@ function om2ax(om) result(ax)
|
||||||
i = maxloc(merge(1.0_pReal,0.0_pReal,cEq(cmplx(Wr,Wi,pReal),cmplx(1.0_pReal,0.0_pReal,pReal),tol=1.0e-14_pReal)),dim=1) ! poor substitute for findloc
|
i = maxloc(merge(1.0_pReal,0.0_pReal,cEq(cmplx(Wr,Wi,pReal),cmplx(1.0_pReal,0.0_pReal,pReal),tol=1.0e-14_pReal)),dim=1) ! poor substitute for findloc
|
||||||
ax(1:3) = VR(1:3,i)
|
ax(1:3) = VR(1:3,i)
|
||||||
where ( dNeq0([om(2,3)-om(3,2), om(3,1)-om(1,3), om(1,2)-om(2,1)])) &
|
where ( dNeq0([om(2,3)-om(3,2), om(3,1)-om(1,3), om(1,2)-om(2,1)])) &
|
||||||
ax(1:3) = sign(ax(1:3),-epsijk *[om(2,3)-om(3,2), om(3,1)-om(1,3), om(1,2)-om(2,1)])
|
ax(1:3) = sign(ax(1:3),-P *[om(2,3)-om(3,2), om(3,1)-om(1,3), om(1,2)-om(2,1)])
|
||||||
endif
|
endif
|
||||||
|
|
||||||
end function om2ax
|
end function om2ax
|
||||||
|
@ -619,7 +619,7 @@ pure function ax2ro(ax) result(ro)
|
||||||
real(pReal), parameter :: thr = 1.0E-7
|
real(pReal), parameter :: thr = 1.0E-7
|
||||||
|
|
||||||
if (dEq0(ax(4))) then
|
if (dEq0(ax(4))) then
|
||||||
ro = [ 0.0_pReal, 0.0_pReal, epsijk, 0.0_pReal ]
|
ro = [ 0.0_pReal, 0.0_pReal, P, 0.0_pReal ]
|
||||||
else
|
else
|
||||||
ro(1:3) = ax(1:3)
|
ro(1:3) = ax(1:3)
|
||||||
! we need to deal with the 180 degree case
|
! we need to deal with the 180 degree case
|
||||||
|
@ -709,7 +709,7 @@ pure function qu2om(qu) result(om)
|
||||||
om(3,2) = 2.0*(qu%z*qu%y+qu%w*qu%x)
|
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,3) = 2.0*(qu%x*qu%z+qu%w*qu%y)
|
||||||
|
|
||||||
if (epsijk < 0.0) om = transpose(om)
|
if (P < 0.0) om = transpose(om)
|
||||||
|
|
||||||
end function qu2om
|
end function qu2om
|
||||||
|
|
||||||
|
@ -734,7 +734,7 @@ function om2qu(om) result(qu)
|
||||||
-om(1,1) +om(2,2) -om(3,3) +1.0_pReal, &
|
-om(1,1) +om(2,2) -om(3,3) +1.0_pReal, &
|
||||||
-om(1,1) -om(2,2) +om(3,3) +1.0_pReal]
|
-om(1,1) -om(2,2) +om(3,3) +1.0_pReal]
|
||||||
|
|
||||||
qu_A = sqrt(max(s,0.0_pReal))*0.5_pReal*[1.0_pReal,epsijk,epsijk,epsijk]
|
qu_A = sqrt(max(s,0.0_pReal))*0.5_pReal*[1.0_pReal,P,P,P]
|
||||||
qu_A = qu_A/norm2(qu_A)
|
qu_A = qu_A/norm2(qu_A)
|
||||||
|
|
||||||
if(any(dEq(abs(qu_A),1.0_pReal,1.0e-15_pReal))) &
|
if(any(dEq(abs(qu_A),1.0_pReal,1.0e-15_pReal))) &
|
||||||
|
@ -803,7 +803,7 @@ pure function qu2ro(qu) result(ro)
|
||||||
ro = [qu%x, qu%y, qu%z, IEEE_value(ro(4),IEEE_positive_inf)]
|
ro = [qu%x, qu%y, qu%z, IEEE_value(ro(4),IEEE_positive_inf)]
|
||||||
else
|
else
|
||||||
s = norm2([qu%x,qu%y,qu%z])
|
s = norm2([qu%x,qu%y,qu%z])
|
||||||
ro = merge ( [ 0.0_pReal, 0.0_pReal, epsijk, 0.0_pReal], &
|
ro = merge ( [ 0.0_pReal, 0.0_pReal, P, 0.0_pReal], &
|
||||||
[ qu%x/s, qu%y/s, qu%z/s, tan(acos(qu%w))], &
|
[ qu%x/s, qu%y/s, qu%z/s, tan(acos(qu%w))], &
|
||||||
s < thr) !ToDo: not save (PGI compiler)
|
s < thr) !ToDo: not save (PGI compiler)
|
||||||
end if
|
end if
|
||||||
|
|
Loading…
Reference in New Issue