vectorized

This commit is contained in:
Martin Diehl 2020-05-19 07:35:58 +02:00
parent b6eebcd704
commit 1a3a4a800e
1 changed files with 32 additions and 22 deletions

View File

@ -629,28 +629,38 @@ class Rotation:
This formulation is from www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion. This formulation is from www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion.
The original formulation had issues. The original formulation had issues.
""" """
def x(a): trace = om[...,0,0:1]+om[...,1,1:2]+om[...,2,2:3]
trace = a[0,0] + a[1,1] + a[2,2]
if trace > 0: with np.errstate(invalid='ignore',divide='ignore'):
s = 0.5 / np.sqrt(trace+ 1.0) s = [
qu = np.array([0.25 / s,( a[2,1] - a[1,2] ) * s,( a[0,2] - a[2,0] ) * s,( a[1,0] - a[0,1] ) * s]) 0.5 / np.sqrt( 1.0 + trace),
else: 2.0 * np.sqrt( 1.0 + om[...,0,0:1] - om[...,1,1:2] - om[...,2,2:3]),
if ( a[0,0] > a[1,1] and a[0,0] > a[2,2] ): 2.0 * np.sqrt( 1.0 + om[...,1,1:2] - om[...,2,2:3] - om[...,0,0:1]),
s = 2.0 * np.sqrt( 1.0 + a[0,0] - a[1,1] - a[2,2]) 2.0 * np.sqrt( 1.0 + om[...,2,2:3] - om[...,0,0:1] - om[...,1,1:2] )
qu = np.array([ (a[2,1] - a[1,2]) / s,0.25 * s,(a[0,1] + a[1,0]) / s,(a[0,2] + a[2,0]) / s]) ]
elif (a[1,1] > a[2,2]): qu= np.where(trace>0,
s = 2.0 * np.sqrt( 1.0 + a[1,1] - a[0,0] - a[2,2]) np.block([0.25 / s[0],
qu = np.array([ (a[0,2] - a[2,0]) / s,(a[0,1] + a[1,0]) / s,0.25 * s,(a[1,2] + a[2,1]) / s]) (om[...,2,1:2] - om[...,1,2:3] ) * s[0],
else: (om[...,0,2:3] - om[...,2,0:1] ) * s[0],
s = 2.0 * np.sqrt( 1.0 + a[2,2] - a[0,0] - a[1,1] ) (om[...,1,0:1] - om[...,0,1:2] ) * s[0]]),
qu = np.array([ (a[1,0] - a[0,1]) / s,(a[0,2] + a[2,0]) / s,(a[1,2] + a[2,1]) / s,0.25 * s]) np.where(om[...,0,0:1] > np.maximum(om[...,1,1:2],om[...,2,2:3]),
np.block([(om[...,2,1:2] - om[...,1,2:3]) / s[1],
0.25 * s[1],
(om[...,0,1:2] + om[...,1,0:1]) / s[1],
(om[...,0,2:3] + om[...,2,0:1]) / s[1]]),
np.where(om[...,1,1:2] > om[...,2,2:3],
np.block([(om[...,0,2:3] - om[...,2,0:1]) / s[2],
(om[...,0,1:2] + om[...,1,0:1]) / s[2],
0.25 * s[2],
(om[...,1,2:3] + om[...,2,1:2]) / s[2]]),
np.block([(om[...,1,0:1] - om[...,0,1:2]) / s[3],
(om[...,0,2:3] + om[...,2,0:1]) / s[3],
(om[...,1,2:3] + om[...,2,1:2]) / s[3],
0.25 * s[3]]),
)
)
)*np.array([1,_P,_P,_P])
return qu return qu
if len(om.shape) > 2:
om_ = om.reshape(-1,3,3)
qu = np.array([x(o) for o in om_]).reshape(om.shape[:-2]+(4,))
else:
qu = x(om)
return qu*np.array([1,_P,_P,_P])
@staticmethod @staticmethod
def om2eu(om): def om2eu(om):