use the formula from the paper, not from the reference implementation

a few multiplications should be faster than a transpose
This commit is contained in:
Martin Diehl 2020-05-18 15:54:58 +02:00
parent a25dd1c438
commit 3dc90ddb94
1 changed files with 8 additions and 8 deletions

View File

@ -534,16 +534,16 @@ class Rotation:
def qu2om(qu): def qu2om(qu):
qq = qu[...,0:1]**2-(qu[...,1:2]**2 + qu[...,2:3]**2 + qu[...,3:4]**2) qq = qu[...,0:1]**2-(qu[...,1:2]**2 + qu[...,2:3]**2 + qu[...,3:4]**2)
om = np.block([qq + 2.0*qu[...,1:2]**2, om = np.block([qq + 2.0*qu[...,1:2]**2,
2.0*(qu[...,2:3]*qu[...,1:2]+qu[...,0:1]*qu[...,3:4]), 2.0*(qu[...,2:3]*qu[...,1:2]+_P*qu[...,0:1]*qu[...,3:4]),
2.0*(qu[...,3:4]*qu[...,1:2]-qu[...,0:1]*qu[...,2:3]), 2.0*(qu[...,3:4]*qu[...,1:2]-_P*qu[...,0:1]*qu[...,2:3]),
2.0*(qu[...,1:2]*qu[...,2:3]-qu[...,0:1]*qu[...,3:4]), 2.0*(qu[...,1:2]*qu[...,2:3]-_P*qu[...,0:1]*qu[...,3:4]),
qq + 2.0*qu[...,2:3]**2, qq + 2.0*qu[...,2:3]**2,
2.0*(qu[...,3:4]*qu[...,2:3]+qu[...,0:1]*qu[...,1:2]), 2.0*(qu[...,3:4]*qu[...,2:3]+_P*qu[...,0:1]*qu[...,1:2]),
2.0*(qu[...,1:2]*qu[...,3:4]+qu[...,0:1]*qu[...,2:3]), 2.0*(qu[...,1:2]*qu[...,3:4]+_P*qu[...,0:1]*qu[...,2:3]),
2.0*(qu[...,2:3]*qu[...,3:4]-qu[...,0:1]*qu[...,1:2]), 2.0*(qu[...,2:3]*qu[...,3:4]-_P*qu[...,0:1]*qu[...,1:2]),
qq + 2.0*qu[...,3:4]**2, qq + 2.0*qu[...,3:4]**2,
]).reshape(qu.shape[:-1]+(3,3)) ]).reshape(qu.shape[:-1]+(3,3))
return om if _P < 0.0 else np.swapaxes(om,(-1,-2)) return om
@staticmethod @staticmethod
def qu2eu(qu): def qu2eu(qu):
@ -641,7 +641,7 @@ class Rotation:
np.zeros(om.shape[:-2]+(1,)), np.zeros(om.shape[:-2]+(1,)),
]), ]),
np.block([np.arctan2(om[...,2,0:1]*zeta,-om[...,2,1:2]*zeta), np.block([np.arctan2(om[...,2,0:1]*zeta,-om[...,2,1:2]*zeta),
np.arccos(om[...,2,2:3]), np.arccos( om[...,2,2:3]),
np.arctan2(om[...,0,2:3]*zeta,+om[...,1,2:3]*zeta) np.arctan2(om[...,0,2:3]*zeta,+om[...,1,2:3]*zeta)
]) ])
) )