closer to thoroughly tested python code

but with extra safety measures to prevent invalid operations
This commit is contained in:
Martin Diehl 2020-07-14 06:55:40 +02:00
parent b0e06f180a
commit 2b58b3df97
1 changed files with 3 additions and 5 deletions

View File

@ -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