WIP: implementing orientation matrix conversions
This commit is contained in:
parent
da30fb8396
commit
43e7639f77
|
@ -596,19 +596,27 @@ class Rotation:
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def om2eu(om):
|
def om2eu(om):
|
||||||
"""Rotation matrix to Bunge-Euler angles."""
|
"""Rotation matrix to Bunge-Euler angles."""
|
||||||
if not np.isclose(np.abs(om[2,2]),1.0,1.e-4):
|
if len(om.shape) == 2:
|
||||||
zeta = 1.0/np.sqrt(1.0-om[2,2]**2)
|
if not np.isclose(np.abs(om[2,2]),1.0,1.e-4):
|
||||||
eu = np.array([np.arctan2(om[2,0]*zeta,-om[2,1]*zeta),
|
zeta = 1.0/np.sqrt(1.0-om[2,2]**2)
|
||||||
np.arccos(om[2,2]),
|
eu = np.array([np.arctan2(om[2,0]*zeta,-om[2,1]*zeta),
|
||||||
np.arctan2(om[0,2]*zeta, om[1,2]*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
|
||||||
else:
|
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
|
with np.errstate(divide='ignore'):
|
||||||
|
zeta = 1.0/np.sqrt(1.0-om[...,2,2:3]**2)
|
||||||
# reduce Euler angles to definition range, i.e a lower limit of 0.0
|
eu = np.block([np.arctan2(om[...,2,0:1]*zeta,-om[...,2,1:2]*zeta),
|
||||||
|
np.arccos(om[...,2,2:3]),
|
||||||
|
np.arctan2(om[...,0,2:3]*zeta,+om[...,1,2:3]*zeta)
|
||||||
|
])
|
||||||
|
# TODO Special case not implemented!
|
||||||
eu[np.abs(eu)<1.e-6] = 0.0
|
eu[np.abs(eu)<1.e-6] = 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)
|
||||||
return eu
|
return eu
|
||||||
|
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def om2ax(om):
|
def om2ax(om):
|
||||||
"""Rotation matrix to axis angle pair."""
|
"""Rotation matrix to axis angle pair."""
|
||||||
|
|
|
@ -161,6 +161,14 @@ class TestRotation:
|
||||||
for q,c in zip(qu,co):
|
for q,c in zip(qu,co):
|
||||||
assert np.allclose(conversion(q),c)
|
assert np.allclose(conversion(q),c)
|
||||||
|
|
||||||
|
@pytest.mark.parametrize('conversion',[Rotation.om2eu,
|
||||||
|
])
|
||||||
|
def test_matrix_vectorization(self,default,conversion):
|
||||||
|
qu = np.array([rot.asMatrix() for rot in default])
|
||||||
|
co = conversion(qu)
|
||||||
|
for q,c in zip(qu,co):
|
||||||
|
assert np.allclose(conversion(q),c)
|
||||||
|
|
||||||
@pytest.mark.parametrize('conversion',[Rotation.eu2qu,
|
@pytest.mark.parametrize('conversion',[Rotation.eu2qu,
|
||||||
Rotation.eu2om,
|
Rotation.eu2om,
|
||||||
Rotation.eu2ax,
|
Rotation.eu2ax,
|
||||||
|
|
Loading…
Reference in New Issue