avoid FPE exceptions

This commit is contained in:
Martin Diehl 2019-04-17 15:19:41 +02:00
parent a6e6db0559
commit fdd5b93e7c
2 changed files with 8 additions and 7 deletions

View File

@ -1132,14 +1132,14 @@ def om2qu(om):
def om2eu(om):
"""Euler angles to orientation matrix"""
if isone(om[2,2]**2):
eu = np.array([np.arctan2( om[0,1],om[0,0]), np.pi*0.5*(1-om[2,2]),0.0]) # following the paper, not the reference implementation
else:
"""Orientation matrix to Euler angles"""
if abs(om[2,2]) < 1.0:
zeta = 1.0/np.sqrt(1.0-om[2,2]**2)
eu = np.array([np.arctan2(om[2,0]*zeta,-om[2,1]*zeta),
np.arccos(om[2,2]),
np.arctan2(om[0,2]*zeta, om[1,2]*zeta)])
else:
eu = np.array([np.arctan2( om[0,1],om[0,0]), np.pi*0.5*(1-om[2,2]),0.0]) # following the paper, not the reference implementation
# reduce Euler angles to definition range, i.e a lower limit of 0.0
eu = np.where(eu<0, (eu+2.0*np.pi)%np.array([2.0*np.pi,np.pi,2.0*np.pi]),eu)

View File

@ -422,14 +422,15 @@ pure function om2eu(om) result(eu)
real(pReal), dimension(3) :: eu
real(pReal) :: zeta
if (abs(om(3,3))>1.0_pReal) then
eu = [ atan2( om(1,2),om(1,1)), 0.5*PI*(1-om(3,3)),0.0_pReal ]
else
if (abs(om(3,3)) < 1.0_pReal) then
zeta = 1.0_pReal/sqrt(1.0_pReal-om(3,3)**2.0_pReal)
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
acos(om(3,3)), &
atan2(om(1,3)*zeta, om(2,3)*zeta)]
else
eu = [ atan2( om(1,2),om(1,1)), 0.5*PI*(1-om(3,3)),0.0_pReal ]
end if
where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI])
end function om2eu