closer to thoroughly tested python code
but with extra safety measures to prevent invalid operations
This commit is contained in:
parent
b0e06f180a
commit
2b58b3df97
|
@ -582,13 +582,11 @@ pure function om2eu(om) result(eu)
|
|||
real(pReal), dimension(3) :: eu
|
||||
real(pReal) :: zeta
|
||||
|
||||
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)
|
||||
if (dNeq(abs(om(3,3)),1.0_pReal,1.e-8_pReal)) then
|
||||
zeta = 1.0_pReal/sqrt(math_clip(1.0_pReal-om(3,3)**2.0_pReal,1e-64_pReal,1.0_pReal))
|
||||
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
||||
acos(om(3,3)), &
|
||||
acos(math_clip(om(3,3),-1.0_pReal,1.0_pReal)), &
|
||||
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
|
||||
eu = [atan2(om(1,2),om(1,1)), 0.5_pReal*PI*(1.0_pReal-om(3,3)),0.0_pReal ]
|
||||
end if
|
||||
|
|
Loading…
Reference in New Issue