avoid FPE exceptions
This commit is contained in:
parent
a6e6db0559
commit
fdd5b93e7c
|
@ -1132,14 +1132,14 @@ def om2qu(om):
|
||||||
|
|
||||||
|
|
||||||
def om2eu(om):
|
def om2eu(om):
|
||||||
"""Euler angles to orientation matrix"""
|
"""Orientation matrix to Euler angles"""
|
||||||
if isone(om[2,2]**2):
|
if abs(om[2,2]) < 1.0:
|
||||||
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:
|
|
||||||
zeta = 1.0/np.sqrt(1.0-om[2,2]**2)
|
zeta = 1.0/np.sqrt(1.0-om[2,2]**2)
|
||||||
eu = np.array([np.arctan2(om[2,0]*zeta,-om[2,1]*zeta),
|
eu = np.array([np.arctan2(om[2,0]*zeta,-om[2,1]*zeta),
|
||||||
np.arccos(om[2,2]),
|
np.arccos(om[2,2]),
|
||||||
np.arctan2(om[0,2]*zeta, om[1,2]*zeta)])
|
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
|
# 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)
|
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), dimension(3) :: eu
|
||||||
real(pReal) :: zeta
|
real(pReal) :: zeta
|
||||||
|
|
||||||
if (abs(om(3,3))>1.0_pReal) then
|
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
|
|
||||||
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)]
|
||||||
|
else
|
||||||
|
eu = [ atan2( om(1,2),om(1,1)), 0.5*PI*(1-om(3,3)),0.0_pReal ]
|
||||||
end if
|
end if
|
||||||
|
|
||||||
where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI])
|
where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI])
|
||||||
|
|
||||||
end function om2eu
|
end function om2eu
|
||||||
|
|
Loading…
Reference in New Issue