using tolerances as in python results in invalid operations
This commit is contained in:
parent
696da9865b
commit
9f79faf819
|
@ -574,6 +574,7 @@ end function om2qu
|
||||||
!---------------------------------------------------------------------------------------------------
|
!---------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief orientation matrix to Euler angles
|
!> @brief orientation matrix to Euler angles
|
||||||
|
!> @details Two step check for special cases to avoid invalid operations (not needed for python)
|
||||||
!---------------------------------------------------------------------------------------------------
|
!---------------------------------------------------------------------------------------------------
|
||||||
pure function om2eu(om) result(eu)
|
pure function om2eu(om) result(eu)
|
||||||
|
|
||||||
|
@ -581,11 +582,13 @@ pure function om2eu(om) result(eu)
|
||||||
real(pReal), dimension(3) :: eu
|
real(pReal), dimension(3) :: eu
|
||||||
real(pReal) :: zeta
|
real(pReal) :: zeta
|
||||||
|
|
||||||
if(dNeq(abs(om(3,3)),1.0_pReal,1.e-9_pReal)) then
|
if (dNeq(abs(om(3,3)),1.0_pReal,1.e-5_pReal)) then
|
||||||
zeta = 1.0_pReal/sqrt(1.0_pReal-om(3,3)**2.0_pReal)
|
zeta = 1.0_pReal/sqrt(1.0_pReal-om(3,3)**2.0_pReal)
|
||||||
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
||||||
acos(om(3,3)), &
|
acos(om(3,3)), &
|
||||||
atan2(om(1,3)*zeta, om(2,3)*zeta)]
|
atan2(om(1,3)*zeta, om(2,3)*zeta)]
|
||||||
|
elseif(dNeq(abs(om(3,3)),1.0_pReal,1.e-8_pReal)) then
|
||||||
|
eu = [PI*.75_pReal,acos(math_clip(om(3,3),-1.0_pReal,1.0_pReal)),PI*.25_pReal]
|
||||||
else
|
else
|
||||||
eu = [atan2(om(1,2),om(1,1)), 0.5_pReal*PI*(1.0_pReal-om(3,3)),0.0_pReal ]
|
eu = [atan2(om(1,2),om(1,1)), 0.5_pReal*PI*(1.0_pReal-om(3,3)),0.0_pReal ]
|
||||||
end if
|
end if
|
||||||
|
|
Loading…
Reference in New Issue