qu(aternion) and eu(ler) vectorized and tested
This commit is contained in:
Martin Diehl
parent
464620b796
commit
da30fb8396
|
|
@ -441,6 +441,7 @@ class Rotation:
|
|||
#---------- Quaternion ----------
|
||||
@staticmethod
|
||||
def qu2om(qu):
|
||||
if len(qu.shape) == 1:
|
||||
"""Quaternion to rotation matrix."""
|
||||
qq = qu[0]**2-(qu[1]**2 + qu[2]**2 + qu[3]**2)
|
||||
om = np.diag(qq + 2.0*np.array([qu[1],qu[2],qu[3]])**2)
|
||||
|
|
@ -452,21 +453,56 @@ class Rotation:
|
|||
om[0,2] = 2.0*(qu[1]*qu[3]+qu[0]*qu[2])
|
||||
om[2,0] = 2.0*(qu[3]*qu[1]-qu[0]*qu[2])
|
||||
return om if P > 0.0 else om.T
|
||||
else:
|
||||
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]),
|
||||
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]),
|
||||
qq + 2.0*qu[...,3:4]**2,
|
||||
]).reshape(qu.shape[:-1]+(3,3))
|
||||
return om # TODO: TRANSPOSE FOR P = 1
|
||||
|
||||
@staticmethod
|
||||
def qu2eu(qu):
|
||||
"""Quaternion to Bunge-Euler angles."""
|
||||
if len(qu.shape) == 1:
|
||||
q03 = qu[0]**2+qu[3]**2
|
||||
q12 = qu[1]**2+qu[2]**2
|
||||
chi = np.sqrt(q03*q12)
|
||||
if np.abs(chi)< 1.e-6:
|
||||
eu = np.array([np.arctan2(-P*2.0*qu[0]*qu[3],qu[0]**2-qu[3]**2), 0.0, 0.0]) if np.abs(q12)< 1.e-6 else \
|
||||
np.array([np.arctan2( 2.0*qu[1]*qu[2],qu[1]**2-qu[2]**2), np.pi, 0.0])
|
||||
if np.abs(q03)< 1.e-6:
|
||||
eu = np.array([np.arctan2(-P*2.0*qu[0]*qu[3],qu[0]**2-qu[3]**2), 0.0, 0.0])
|
||||
elif np.abs(q12)< 1.e-6:
|
||||
eu = np.array([np.arctan2( 2.0*qu[1]*qu[2],qu[1]**2-qu[2]**2), np.pi, 0.0])
|
||||
else:
|
||||
eu = np.array([np.arctan2((-P*qu[0]*qu[2]+qu[1]*qu[3])*chi, (-P*qu[0]*qu[1]-qu[2]*qu[3])*chi ),
|
||||
np.arctan2( 2.0*chi, q03-q12 ),
|
||||
np.arctan2(( P*qu[0]*qu[2]+qu[1]*qu[3])*chi, (-P*qu[0]*qu[1]+qu[2]*qu[3])*chi )])
|
||||
else:
|
||||
q02 = qu[...,0:1]*qu[...,2:3]
|
||||
q13 = qu[...,1:2]*qu[...,3:4]
|
||||
q01 = qu[...,0:1]*qu[...,1:2]
|
||||
q23 = qu[...,2:3]*qu[...,3:4]
|
||||
q03_s = qu[...,0:1]**2+qu[...,3:4]**2
|
||||
q12_s = qu[...,1:2]**2+qu[...,2:3]**2
|
||||
chi = np.sqrt(q03_s*q12_s)
|
||||
|
||||
eu = np.where(np.abs(q12_s) < 1.0e-6,
|
||||
np.block([np.arctan2(-P*2.0*qu[...,0:1]*qu[...,3:4],qu[...,0:1]**2-qu[...,3:4]**2),
|
||||
np.zeros(qu.shape[:-1]+(2,))]),
|
||||
np.block([np.arctan2((-P*q02+q13)*chi, (-P*q01-q23)*chi),
|
||||
np.arctan2( 2.0*chi, q03_s-q12_s ),
|
||||
np.arctan2(( P*q02+q13)*chi, (-P*q01+q23)*chi)])
|
||||
)
|
||||
eu = np.where(np.abs(q03_s) < 1.0e-6,
|
||||
np.block([np.arctan2( 2.0*qu[...,1:2]*qu[...,2:3],qu[...,1:2]**2-qu[...,2:3]**2),
|
||||
np.ones( qu.shape[:-1]+(1,))*np.pi,
|
||||
np.zeros(qu.shape[:-1]+(1,))]),
|
||||
eu) # TODO: Where not needed
|
||||
# reduce Euler angles to definition range, i.e a lower limit of 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)
|
||||
|
|
@ -479,38 +515,66 @@ class Rotation:
|
|||
|
||||
Modified version of the original formulation, should be numerically more stable
|
||||
"""
|
||||
if iszero(qu[1]**2+qu[2]**2+qu[3]**2): # set axis to [001] if the angle is 0/360
|
||||
ax = [ 0.0, 0.0, 1.0, 0.0 ]
|
||||
if len(qu.shape) == 1:
|
||||
if iszero(np.sum(qu[1:4]**2)): # set axis to [001] if the angle is 0/360
|
||||
ax = np.array([ 0.0, 0.0, 1.0, 0.0 ])
|
||||
elif np.abs(qu[0]) > 1.e-6:
|
||||
s = np.sign(qu[0])/np.sqrt(qu[1]**2+qu[2]**2+qu[3]**2)
|
||||
omega = 2.0 * np.arccos(np.clip(qu[0],-1.0,1.0))
|
||||
ax = [ qu[1]*s, qu[2]*s, qu[3]*s, omega ]
|
||||
ax = ax = np.array([ qu[1]*s, qu[2]*s, qu[3]*s, omega ])
|
||||
else:
|
||||
ax = [ qu[1], qu[2], qu[3], np.pi]
|
||||
return np.array(ax)
|
||||
ax = ax = np.array([ qu[1], qu[2], qu[3], np.pi])
|
||||
else:
|
||||
with np.errstate(divide='ignore'):
|
||||
s = np.sign(qu[...,0:1])/np.sqrt(qu[...,1:2]**2+qu[...,2:3]**2+qu[...,3:4]**2)
|
||||
omega = 2.0 * np.arccos(np.clip(qu[...,0:1],-1.0,1.0))
|
||||
|
||||
ax = np.where(qu[...,0:1] < 1.0e-6,
|
||||
np.block([qu[...,1:4],np.ones(qu.shape[:-1]+(1,))*np.pi]),
|
||||
np.block([qu[...,1:4]*s,omega]))
|
||||
ax = np.where(np.expand_dims(np.sum(np.abs(qu[:,1:4])**2,axis=-1) < 1.0e-6,-1),
|
||||
[0.0, 0.0, 1.0, 0.0], ax) # TODO: Where not needed
|
||||
return ax
|
||||
|
||||
|
||||
@staticmethod
|
||||
def qu2ro(qu):
|
||||
"""Quaternion to Rodrigues-Frank vector."""
|
||||
if len(qu.shape) == 1:
|
||||
if iszero(qu[0]):
|
||||
ro = [qu[1], qu[2], qu[3], np.inf]
|
||||
ro = np.array([qu[1], qu[2], qu[3], np.inf])
|
||||
else:
|
||||
s = np.linalg.norm([qu[1],qu[2],qu[3]])
|
||||
ro = [0.0,0.0,P,0.0] if iszero(s) else \
|
||||
[ qu[1]/s, qu[2]/s, qu[3]/s, np.tan(np.arccos(np.clip(qu[0],-1.0,1.0)))]
|
||||
|
||||
return np.array(ro)
|
||||
ro = np.array([0.0,0.0,P,0.0] if iszero(s) else \
|
||||
[ qu[1]/s, qu[2]/s, qu[3]/s, np.tan(np.arccos(np.clip(qu[0],-1.0,1.0)))])
|
||||
else:
|
||||
s = np.expand_dims(np.linalg.norm(qu[...,1:4],axis=1),-1)
|
||||
ro = np.where(np.abs(s) < 1.0e-12,
|
||||
[0.0,0.0,P,0.0],
|
||||
np.block([qu[...,1:2]/s,qu[...,2:3]/s,qu[...,3:4]/s,
|
||||
np.tan(np.arccos(np.clip(qu[:,0:1],-1.0,1.0)))
|
||||
])
|
||||
)
|
||||
ro = np.where(np.abs(qu[...,0:1]) < 1.0e-12,
|
||||
np.block([qu[...,1:2], qu[...,2:3], qu[...,3:4], np.ones(qu.shape[:-1]+(1,))*np.inf]),ro) # TODO: Where not needed
|
||||
return ro
|
||||
|
||||
@staticmethod
|
||||
def qu2ho(qu):
|
||||
"""Quaternion to homochoric vector."""
|
||||
if len(qu.shape) == 1:
|
||||
if np.isclose(qu[0],1.0):
|
||||
ho = np.array([ 0.0, 0.0, 0.0 ])
|
||||
ho = np.zeros(3)
|
||||
else:
|
||||
omega = 2.0 * np.arccos(np.clip(qu[0],-1.0,1.0))
|
||||
ho = np.array([qu[1], qu[2], qu[3]])
|
||||
f = 0.75 * ( omega - np.sin(omega) )
|
||||
ho = ho/np.linalg.norm(ho) * f**(1./3.)
|
||||
else:
|
||||
omega = 2.0 * np.arccos(np.clip(qu[...,0:1],-1.0,1.0))
|
||||
ho = np.where(np.abs(omega) < 1.0e-12,
|
||||
np.zeros(3),
|
||||
qu[...,1:4]/np.linalg.norm(qu[...,1:4],axis=1).reshape(qu.shape[:-1]+(1,)) * (0.75*(omega - np.sin(omega)))**(1./3.))
|
||||
return ho
|
||||
|
||||
@staticmethod
|
||||
|
|
@ -555,7 +619,7 @@ class Rotation:
|
|||
ax[3] = np.arccos(np.clip(t,-1.0,1.0))
|
||||
|
||||
if np.abs(ax[3])<1.e-6:
|
||||
ax = [ 0.0, 0.0, 1.0, 0.0]
|
||||
ax = np.array([ 0.0, 0.0, 1.0, 0.0])
|
||||
else:
|
||||
w,vr = np.linalg.eig(om)
|
||||
# next, find the eigenvalue (1,0j)
|
||||
|
|
@ -563,7 +627,7 @@ class Rotation:
|
|||
ax[0:3] = np.real(vr[0:3,i])
|
||||
diagDelta = np.array([om[1,2]-om[2,1],om[2,0]-om[0,2],om[0,1]-om[1,0]])
|
||||
ax[0:3] = np.where(np.abs(diagDelta)<1.e-6, ax[0:3],np.abs(ax[0:3])*np.sign(-P*diagDelta))
|
||||
return np.array(ax)
|
||||
return ax
|
||||
|
||||
@staticmethod
|
||||
def om2ro(om):
|
||||
|
|
@ -585,6 +649,7 @@ class Rotation:
|
|||
@staticmethod
|
||||
def eu2qu(eu):
|
||||
"""Bunge-Euler angles to quaternion."""
|
||||
if len(eu.shape) == 1:
|
||||
ee = 0.5*eu
|
||||
cPhi = np.cos(ee[1])
|
||||
sPhi = np.sin(ee[1])
|
||||
|
|
@ -593,24 +658,48 @@ class Rotation:
|
|||
-P*sPhi*np.sin(ee[0]-ee[2]),
|
||||
-P*cPhi*np.sin(ee[0]+ee[2]) ])
|
||||
if qu[0] < 0.0: qu*=-1
|
||||
else:
|
||||
ee = 0.5*eu
|
||||
cPhi = np.cos(ee[...,1:2])
|
||||
sPhi = np.sin(ee[...,1:2])
|
||||
qu = np.block([ cPhi*np.cos(ee[...,0:1]+ee[...,2:3]),
|
||||
-P*sPhi*np.cos(ee[...,0:1]-ee[...,2:3]),
|
||||
-P*sPhi*np.sin(ee[...,0:1]-ee[...,2:3]),
|
||||
-P*cPhi*np.sin(ee[...,0:1]+ee[...,2:3])])
|
||||
qu[qu[...,0]<0.0]*=-1
|
||||
return qu
|
||||
|
||||
|
||||
@staticmethod
|
||||
def eu2om(eu):
|
||||
"""Bunge-Euler angles to rotation matrix."""
|
||||
if len(eu.shape) == 1:
|
||||
c = np.cos(eu)
|
||||
s = np.sin(eu)
|
||||
|
||||
om = np.array([[+c[0]*c[2]-s[0]*s[2]*c[1], +s[0]*c[2]+c[0]*s[2]*c[1], +s[2]*s[1]],
|
||||
[-c[0]*s[2]-s[0]*c[2]*c[1], -s[0]*s[2]+c[0]*c[2]*c[1], +c[2]*s[1]],
|
||||
[+s[0]*s[1], -c[0]*s[1], +c[1] ]])
|
||||
|
||||
om[np.abs(om)<1.e-6] = 0.0
|
||||
else:
|
||||
c = np.cos(eu)
|
||||
s = np.sin(eu)
|
||||
om = np.block([+c[...,0:1]*c[...,2:3]-s[...,0:1]*s[...,2:3]*c[...,1:2],
|
||||
+s[...,0:1]*c[...,2:3]+c[...,0:1]*s[...,2:3]*c[...,1:2],
|
||||
+s[...,2:3]*s[...,1:2],
|
||||
-c[...,0:1]*s[...,2:3]-s[...,0:1]*c[...,2:3]*c[...,1:2],
|
||||
-s[...,0:1]*s[...,2:3]+c[...,0:1]*c[...,2:3]*c[...,1:2],
|
||||
+c[...,2:3]*s[...,1:2],
|
||||
+s[...,0:1]*s[...,1:2],
|
||||
-c[...,0:1]*s[...,1:2],
|
||||
+c[...,1:2]
|
||||
]).reshape(eu.shape[:-1]+(3,3))
|
||||
om[np.abs(om)<1.e-12] = 0.0
|
||||
return om
|
||||
|
||||
@staticmethod
|
||||
def eu2ax(eu):
|
||||
"""Bunge-Euler angles to axis angle pair."""
|
||||
if len(eu.shape) == 1:
|
||||
t = np.tan(eu[1]*0.5)
|
||||
sigma = 0.5*(eu[0]+eu[2])
|
||||
delta = 0.5*(eu[0]-eu[2])
|
||||
|
|
@ -624,11 +713,26 @@ class Rotation:
|
|||
ax = -P/tau * np.array([ t*np.cos(delta), t*np.sin(delta), np.sin(sigma) ]) # passive axis angle pair so a minus sign in front
|
||||
ax = np.append(ax,alpha)
|
||||
if alpha < 0.0: ax *= -1.0 # ensure alpha is positive
|
||||
else:
|
||||
t = np.tan(eu[...,1:2]*0.5)
|
||||
sigma = 0.5*(eu[...,0:1]+eu[...,2:3])
|
||||
delta = 0.5*(eu[...,0:1]-eu[...,2:3])
|
||||
tau = np.linalg.norm(np.block([t,np.sin(sigma)]),axis=-1).reshape(-1,1)
|
||||
alpha = np.where(np.abs(np.cos(sigma))<1.e-12,np.pi,2.0*np.arctan(tau/np.cos(sigma)))
|
||||
ax = np.where(np.broadcast_to(np.abs(alpha)<1.0e-12,eu.shape[:-1]+(4,)),
|
||||
[0.0,0.0,1.0,0.0],
|
||||
np.block([-P/tau*t*np.cos(delta),
|
||||
-P/tau*t*np.sin(delta),
|
||||
-P/tau* np.sin(sigma),
|
||||
alpha
|
||||
]))
|
||||
ax[(alpha<0.0).squeeze()] *=-1
|
||||
return ax
|
||||
|
||||
@staticmethod
|
||||
def eu2ro(eu):
|
||||
"""Bunge-Euler angles to Rodrigues-Frank vector."""
|
||||
if len(eu.shape) == 1:
|
||||
ro = Rotation.eu2ax(eu) # convert to axis angle pair representation
|
||||
if ro[3] >= np.pi: # Differs from original implementation. check convention 5
|
||||
ro[3] = np.inf
|
||||
|
|
@ -636,6 +740,11 @@ class Rotation:
|
|||
ro = np.array([ 0.0, 0.0, P, 0.0 ])
|
||||
else:
|
||||
ro[3] = np.tan(ro[3]*0.5)
|
||||
else:
|
||||
ax = Rotation.eu2ax(eu)
|
||||
ro = np.block([ax[:,:3],np.tan(ax[:,3:4]*.5)])
|
||||
ro[ax[:,3]>=np.pi,3] = np.inf
|
||||
ro[np.abs(ax[:,3])<1.e-16] = [ 0.0, 0.0, P, 0.0 ]
|
||||
return ro
|
||||
|
||||
@staticmethod
|
||||
|
|
@ -664,9 +773,7 @@ class Rotation:
|
|||
else:
|
||||
c = np.cos(ax[...,3:4]*.5)
|
||||
s = np.sin(ax[...,3:4]*.5)
|
||||
qu = np.where(np.abs(ax[...,3:4])<1.e-12,
|
||||
[1.0, 0.0, 0.0, 0.0],
|
||||
np.block([c, ax[...,:3]*s]))
|
||||
qu = np.where(np.abs(ax[...,3:4])<1.e-12,[1.0, 0.0, 0.0, 0.0],np.block([c, ax[...,:3]*s]))
|
||||
return qu
|
||||
|
||||
@staticmethod
|
||||
|
|
@ -687,7 +794,7 @@ class Rotation:
|
|||
c = np.cos(ax[...,3:4])
|
||||
s = np.sin(ax[...,3:4])
|
||||
omc = 1. -c
|
||||
ax = np.block([c+omc*ax[...,0:1]**2,
|
||||
om = np.block([c+omc*ax[...,0:1]**2,
|
||||
omc*ax[...,0:1]*ax[...,1:2] + s*ax[...,2:3],
|
||||
omc*ax[...,0:1]*ax[...,2:3] - s*ax[...,1:2],
|
||||
omc*ax[...,0:1]*ax[...,1:2] - s*ax[...,2:3],
|
||||
|
|
@ -696,7 +803,7 @@ class Rotation:
|
|||
omc*ax[...,0:1]*ax[...,2:3] + s*ax[...,1:2],
|
||||
omc*ax[...,1:2]*ax[...,2:3] - s*ax[...,0:1],
|
||||
c+omc*ax[...,2:3]**2]).reshape(ax.shape[:-1]+(3,3))
|
||||
return ax
|
||||
return om # TODO: TRANSPOSE FOR P = 1
|
||||
|
||||
@staticmethod
|
||||
def ax2eu(ax):
|
||||
|
|
|
|||
|
|
@ -150,6 +150,28 @@ class TestRotation:
|
|||
print(m,o,rot.asQuaternion())
|
||||
assert np.allclose(m,o,atol=atol)
|
||||
|
||||
@pytest.mark.parametrize('conversion',[Rotation.qu2om,
|
||||
Rotation.qu2eu,
|
||||
Rotation.qu2ax,
|
||||
Rotation.qu2ro,
|
||||
Rotation.qu2ho])
|
||||
def test_quaternion_vectorization(self,default,conversion):
|
||||
qu = np.array([rot.asQuaternion() 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,
|
||||
Rotation.eu2ro,
|
||||
])
|
||||
def test_Euler_vectorization(self,default,conversion):
|
||||
qu = np.array([rot.asEulers() 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.ax2qu,
|
||||
Rotation.ax2om,
|
||||
Rotation.ax2ro,
|
||||
|
|
|
|||
Loading…
Reference in New Issue