avoid FPE exceptions
This commit is contained in:
parent
a6e6db0559
commit
fdd5b93e7c
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue