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,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."""

View File

@ -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,