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):
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,
2.0*(qu[...,2:3]*qu[...,1:2]+qu[...,0:1]*qu[...,3:4]),
2.0*(qu[...,3:4]*qu[...,1:2]-qu[...,0:1]*qu[...,2:3]),
2.0*(qu[...,1:2]*qu[...,2:3]-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]-_P*qu[...,0:1]*qu[...,2:3]),
2.0*(qu[...,1:2]*qu[...,2:3]-_P*qu[...,0:1]*qu[...,3:4]),
qq + 2.0*qu[...,2:3]**2,
2.0*(qu[...,3:4]*qu[...,2:3]+qu[...,0:1]*qu[...,1:2]),
2.0*(qu[...,1:2]*qu[...,3:4]+qu[...,0:1]*qu[...,2:3]),
2.0*(qu[...,2:3]*qu[...,3:4]-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]+_P*qu[...,0:1]*qu[...,2:3]),
2.0*(qu[...,2:3]*qu[...,3:4]-_P*qu[...,0:1]*qu[...,1:2]),
qq + 2.0*qu[...,3:4]**2,
]).reshape(qu.shape[:-1]+(3,3))
return om if _P < 0.0 else np.swapaxes(om,(-1,-2))
return om
@staticmethod
def qu2eu(qu):
@ -641,7 +641,7 @@ class Rotation:
np.zeros(om.shape[:-2]+(1,)),
]),
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)
])
)