WIP: implementing orientation matrix conversions

This commit is contained in:
Martin Diehl 2020-04-09 00:47:43 +02:00
parent da30fb8396
commit 43e7639f77
2 changed files with 24 additions and 8 deletions

View File

@ -596,6 +596,7 @@ class Rotation:
@staticmethod
def om2eu(om):
"""Rotation matrix to Bunge-Euler angles."""
if len(om.shape) == 2:
if not np.isclose(np.abs(om[2,2]),1.0,1.e-4):
zeta = 1.0/np.sqrt(1.0-om[2,2]**2)
eu = np.array([np.arctan2(om[2,0]*zeta,-om[2,1]*zeta),
@ -603,12 +604,19 @@ class Rotation:
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
else:
with np.errstate(divide='ignore'):
zeta = 1.0/np.sqrt(1.0-om[...,2,2:3]**2)
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.where(eu<0, (eu+2.0*np.pi)%np.array([2.0*np.pi,np.pi,2.0*np.pi]),eu)
return eu
@staticmethod
def om2ax(om):
"""Rotation matrix to axis angle pair."""

View File

@ -161,6 +161,14 @@ class TestRotation:
for q,c in zip(qu,co):
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,
Rotation.eu2om,
Rotation.eu2ax,