parent
3a7408a213
commit
d842902a76
|
@ -1247,6 +1247,20 @@ def iszero(a):
|
||||||
|
|
||||||
#---------- quaternion ----------
|
#---------- quaternion ----------
|
||||||
|
|
||||||
|
def qu2om(qu):
|
||||||
|
"""Quaternion to orientation 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)
|
||||||
|
|
||||||
|
om[1,0] = 2.0*(qu[2]*qu[1]+qu[0]*qu[3])
|
||||||
|
om[0,1] = 2.0*(qu[1]*qu[2]-qu[0]*qu[3])
|
||||||
|
om[2,1] = 2.0*(qu[3]*qu[2]+qu[0]*qu[1])
|
||||||
|
om[1,2] = 2.0*(qu[2]*qu[3]-qu[0]*qu[1])
|
||||||
|
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
|
||||||
|
|
||||||
|
|
||||||
def qu2eu(qu):
|
def qu2eu(qu):
|
||||||
"""Quaternion to Euler angles"""
|
"""Quaternion to Euler angles"""
|
||||||
q03 = qu[0]**2+qu[3]**2
|
q03 = qu[0]**2+qu[3]**2
|
||||||
|
@ -1265,19 +1279,6 @@ def qu2eu(qu):
|
||||||
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
|
||||||
|
|
||||||
def qu2om(qu):
|
|
||||||
"""Quaternion to orientation 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)
|
|
||||||
|
|
||||||
om[1,0] = 2.0*(qu[2]*qu[1]+qu[0]*qu[3])
|
|
||||||
om[0,1] = 2.0*(qu[1]*qu[2]-qu[0]*qu[3])
|
|
||||||
om[2,1] = 2.0*(qu[3]*qu[2]+qu[0]*qu[1])
|
|
||||||
om[1,2] = 2.0*(qu[2]*qu[3]-qu[0]*qu[1])
|
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
def qu2ax(qu):
|
def qu2ax(qu):
|
||||||
"""
|
"""
|
||||||
|
@ -1330,6 +1331,14 @@ def qu2cu(qu):
|
||||||
|
|
||||||
#---------- orientation matrix ----------
|
#---------- orientation matrix ----------
|
||||||
|
|
||||||
|
def om2qu(om):
|
||||||
|
"""
|
||||||
|
Orientation matrix to quaternion
|
||||||
|
|
||||||
|
The original formulation (direct conversion) had numerical issues
|
||||||
|
"""
|
||||||
|
return ax2qu(om2ax(om))
|
||||||
|
|
||||||
|
|
||||||
def om2eu(om):
|
def om2eu(om):
|
||||||
"""Euler angles to orientation matrix"""
|
"""Euler angles to orientation matrix"""
|
||||||
|
@ -1367,32 +1376,35 @@ def om2ax(om):
|
||||||
return np.array(ax)
|
return np.array(ax)
|
||||||
|
|
||||||
|
|
||||||
def om2qu(om):
|
|
||||||
"""
|
|
||||||
Orientation matrix to quaternion
|
|
||||||
|
|
||||||
The original formulation (direct conversion) had numerical issues
|
|
||||||
"""
|
|
||||||
return ax2qu(om2ax(om))
|
|
||||||
|
|
||||||
|
|
||||||
def om2ro(om):
|
def om2ro(om):
|
||||||
"""Orientation matrix to Rodriques vector"""
|
"""Orientation matrix to Rodriques vector"""
|
||||||
return eu2ro(om2eu(om))
|
return eu2ro(om2eu(om))
|
||||||
|
|
||||||
|
|
||||||
def om2cu(om):
|
|
||||||
"""Orientation matrix to cubochoric"""
|
|
||||||
return ho2cu(om2ho(om))
|
|
||||||
|
|
||||||
|
|
||||||
def om2ho(om):
|
def om2ho(om):
|
||||||
"""Orientation matrix to homochoric"""
|
"""Orientation matrix to homochoric"""
|
||||||
return ax2ho(om2ax(om))
|
return ax2ho(om2ax(om))
|
||||||
|
|
||||||
|
|
||||||
|
def om2cu(om):
|
||||||
|
"""Orientation matrix to cubochoric"""
|
||||||
|
return ho2cu(om2ho(om))
|
||||||
|
|
||||||
|
|
||||||
#---------- Euler angles ----------
|
#---------- Euler angles ----------
|
||||||
|
|
||||||
|
def eu2qu(eu):
|
||||||
|
"""Euler angles to quaternion"""
|
||||||
|
ee = 0.5*eu
|
||||||
|
cPhi = np.cos(ee[1])
|
||||||
|
sPhi = np.sin(ee[1])
|
||||||
|
qu = np.array([ cPhi*np.cos(ee[0]+ee[2]),
|
||||||
|
-P*sPhi*np.cos(ee[0]-ee[2]),
|
||||||
|
-P*sPhi*np.sin(ee[0]-ee[2]),
|
||||||
|
-P*cPhi*np.sin(ee[0]+ee[2]) ])
|
||||||
|
#if qu[0] < 0.0: qu.homomorph() !ToDo: Check with original
|
||||||
|
return qu
|
||||||
|
|
||||||
|
|
||||||
def eu2om(eu):
|
def eu2om(eu):
|
||||||
"""Euler angles to orientation matrix"""
|
"""Euler angles to orientation matrix"""
|
||||||
|
@ -1444,19 +1456,6 @@ def eu2ho(eu):
|
||||||
return ax2ho(eu2ax(eu))
|
return ax2ho(eu2ax(eu))
|
||||||
|
|
||||||
|
|
||||||
def eu2qu(eu):
|
|
||||||
"""Euler angles to quaternion"""
|
|
||||||
ee = 0.5*eu
|
|
||||||
cPhi = np.cos(ee[1])
|
|
||||||
sPhi = np.sin(ee[1])
|
|
||||||
qu = np.array([ cPhi*np.cos(ee[0]+ee[2]),
|
|
||||||
-P*sPhi*np.cos(ee[0]-ee[2]),
|
|
||||||
-P*sPhi*np.sin(ee[0]-ee[2]),
|
|
||||||
-P*cPhi*np.sin(ee[0]+ee[2]) ])
|
|
||||||
#if qu[0] < 0.0: qu.homomorph() !ToDo: Check with original
|
|
||||||
return qu
|
|
||||||
|
|
||||||
|
|
||||||
def eu2cu(eu):
|
def eu2cu(eu):
|
||||||
"""Euler angles to cubochoric"""
|
"""Euler angles to cubochoric"""
|
||||||
return ho2cu(eu2ho(eu))
|
return ho2cu(eu2ho(eu))
|
||||||
|
@ -1464,10 +1463,16 @@ def eu2cu(eu):
|
||||||
|
|
||||||
#---------- axis angle ----------
|
#---------- axis angle ----------
|
||||||
|
|
||||||
|
def ax2qu(ax):
|
||||||
|
"""Axis angle to quaternion"""
|
||||||
|
if iszero(ax[3]):
|
||||||
|
qu = np.array([ 1.0, 0.0, 0.0, 0.0 ])
|
||||||
|
else:
|
||||||
|
c = np.cos(ax[3]*0.5)
|
||||||
|
s = np.sin(ax[3]*0.5)
|
||||||
|
qu = np.array([ c, ax[0]*s, ax[1]*s, ax[2]*s ])
|
||||||
|
|
||||||
def ax2eu(ax):
|
return qu
|
||||||
"""Orientation matrix to Euler angles"""
|
|
||||||
return om2eu(ax2om(ax))
|
|
||||||
|
|
||||||
|
|
||||||
def ax2om(ax):
|
def ax2om(ax):
|
||||||
|
@ -1485,6 +1490,11 @@ def ax2om(ax):
|
||||||
return om if P < 0.0 else om.T
|
return om if P < 0.0 else om.T
|
||||||
|
|
||||||
|
|
||||||
|
def ax2eu(ax):
|
||||||
|
"""Orientation matrix to Euler angles"""
|
||||||
|
return om2eu(ax2om(ax))
|
||||||
|
|
||||||
|
|
||||||
def ax2ro(ax):
|
def ax2ro(ax):
|
||||||
"""Axis angle to Rodrigues vector"""
|
"""Axis angle to Rodrigues vector"""
|
||||||
if iszero(ax[3]):
|
if iszero(ax[3]):
|
||||||
|
@ -1498,18 +1508,6 @@ def ax2ro(ax):
|
||||||
return np.array(ro)
|
return np.array(ro)
|
||||||
|
|
||||||
|
|
||||||
def ax2qu(ax):
|
|
||||||
"""Axis angle to quaternion"""
|
|
||||||
if iszero(ax[3]):
|
|
||||||
qu = np.array([ 1.0, 0.0, 0.0, 0.0 ])
|
|
||||||
else:
|
|
||||||
c = np.cos(ax[3]*0.5)
|
|
||||||
s = np.sin(ax[3]*0.5)
|
|
||||||
qu = np.array([ c, ax[0]*s, ax[1]*s, ax[2]*s ])
|
|
||||||
|
|
||||||
return qu
|
|
||||||
|
|
||||||
|
|
||||||
def ax2ho(ax):
|
def ax2ho(ax):
|
||||||
"""Axis angle to homochoric"""
|
"""Axis angle to homochoric"""
|
||||||
f = (0.75 * ( ax[3] - np.sin(ax[3]) ))**(1.0/3.0)
|
f = (0.75 * ( ax[3] - np.sin(ax[3]) ))**(1.0/3.0)
|
||||||
|
@ -1524,6 +1522,20 @@ def ax2cu(ax):
|
||||||
|
|
||||||
#---------- Rodrigues--Frank ----------
|
#---------- Rodrigues--Frank ----------
|
||||||
|
|
||||||
|
def ro2qu(ro):
|
||||||
|
"""Rodrigues vector to quaternion"""
|
||||||
|
return ax2qu(ro2ax(ro))
|
||||||
|
|
||||||
|
|
||||||
|
def ro2om(ro):
|
||||||
|
"""Rodgrigues vector to orientation matrix"""
|
||||||
|
return ax2om(ro2ax(ro))
|
||||||
|
|
||||||
|
|
||||||
|
def ro2eu(ro):
|
||||||
|
"""Rodrigues vector to orientation matrix"""
|
||||||
|
return om2eu(ro2om(ro))
|
||||||
|
|
||||||
|
|
||||||
def ro2ax(ro):
|
def ro2ax(ro):
|
||||||
"""Rodrigues vector to axis angle"""
|
"""Rodrigues vector to axis angle"""
|
||||||
|
@ -1541,21 +1553,6 @@ def ro2ax(ro):
|
||||||
return np.array(ax)
|
return np.array(ax)
|
||||||
|
|
||||||
|
|
||||||
def ro2eu(ro):
|
|
||||||
"""Rodrigues vector to orientation matrix"""
|
|
||||||
return om2eu(ro2om(ro))
|
|
||||||
|
|
||||||
|
|
||||||
def ro2om(ro):
|
|
||||||
"""Rodgrigues vector to orientation matrix"""
|
|
||||||
return ax2om(ro2ax(ro))
|
|
||||||
|
|
||||||
|
|
||||||
def ro2qu(ro):
|
|
||||||
"""Rodrigues vector to quaternion"""
|
|
||||||
return ax2qu(ro2ax(ro))
|
|
||||||
|
|
||||||
|
|
||||||
def ro2ho(ro):
|
def ro2ho(ro):
|
||||||
"""Rodrigues vector to homochoric"""
|
"""Rodrigues vector to homochoric"""
|
||||||
if iszero(np.sum(ro[0:3]**2.0)):
|
if iszero(np.sum(ro[0:3]**2.0)):
|
||||||
|
@ -1574,6 +1571,20 @@ def ro2cu(ro):
|
||||||
|
|
||||||
#---------- homochoric ----------
|
#---------- homochoric ----------
|
||||||
|
|
||||||
|
def ho2qu(ho):
|
||||||
|
"""Homochoric to quaternion"""
|
||||||
|
return ax2qu(ho2ax(ho))
|
||||||
|
|
||||||
|
|
||||||
|
def ho2om(ho):
|
||||||
|
"""Homochoric to orientation matrix"""
|
||||||
|
return ax2om(ho2ax(ho))
|
||||||
|
|
||||||
|
|
||||||
|
def ho2eu(ho):
|
||||||
|
"""Homochoric to Euler angles"""
|
||||||
|
return ax2eu(ho2ax(ho))
|
||||||
|
|
||||||
|
|
||||||
def ho2ax(ho):
|
def ho2ax(ho):
|
||||||
"""Homochoric to axis angle"""
|
"""Homochoric to axis angle"""
|
||||||
|
@ -1601,34 +1612,21 @@ def ho2ax(ho):
|
||||||
return ax
|
return ax
|
||||||
|
|
||||||
|
|
||||||
def ho2cu(ho):
|
|
||||||
"""Homochoric to cubochoric"""
|
|
||||||
return Lambert.BallToCube(ho)
|
|
||||||
|
|
||||||
|
|
||||||
def ho2eu(ho):
|
|
||||||
"""Homochoric to Euler angles"""
|
|
||||||
return ax2eu(ho2ax(ho))
|
|
||||||
|
|
||||||
|
|
||||||
def ho2om(ho):
|
|
||||||
"""Homochoric to orientation matrix"""
|
|
||||||
return ax2om(ho2ax(ho))
|
|
||||||
|
|
||||||
|
|
||||||
def ho2ro(ho):
|
def ho2ro(ho):
|
||||||
"""Axis angle to Rodriques vector"""
|
"""Axis angle to Rodriques vector"""
|
||||||
return ax2ro(ho2ax(ho))
|
return ax2ro(ho2ax(ho))
|
||||||
|
|
||||||
|
|
||||||
def ho2qu(ho):
|
def ho2cu(ho):
|
||||||
"""Homochoric to quaternion"""
|
"""Homochoric to cubochoric"""
|
||||||
return ax2qu(ho2ax(ho))
|
return Lambert.BallToCube(ho)
|
||||||
|
|
||||||
|
|
||||||
def cu2eu(cu):
|
#---------- cubochoric ----------
|
||||||
"""Cubochoric to Euler angles"""
|
|
||||||
return ho2eu(cu2ho(cu))
|
def cu2qu(cu):
|
||||||
|
"""Cubochoric to quaternion"""
|
||||||
|
return ho2qu(cu2ho(cu))
|
||||||
|
|
||||||
|
|
||||||
def cu2om(cu):
|
def cu2om(cu):
|
||||||
|
@ -1636,6 +1634,11 @@ def cu2om(cu):
|
||||||
return ho2om(cu2ho(cu))
|
return ho2om(cu2ho(cu))
|
||||||
|
|
||||||
|
|
||||||
|
def cu2eu(cu):
|
||||||
|
"""Cubochoric to Euler angles"""
|
||||||
|
return ho2eu(cu2ho(cu))
|
||||||
|
|
||||||
|
|
||||||
def cu2ax(cu):
|
def cu2ax(cu):
|
||||||
"""Cubochoric to axis angle"""
|
"""Cubochoric to axis angle"""
|
||||||
return ho2ax(cu2ho(cu))
|
return ho2ax(cu2ho(cu))
|
||||||
|
@ -1646,13 +1649,6 @@ def cu2ro(cu):
|
||||||
return ho2ro(cu2ho(cu))
|
return ho2ro(cu2ho(cu))
|
||||||
|
|
||||||
|
|
||||||
def cu2qu(cu):
|
|
||||||
"""Cubochoric to quaternion"""
|
|
||||||
return ho2qu(cu2ho(cu))
|
|
||||||
|
|
||||||
|
|
||||||
def cu2ho(cu):
|
def cu2ho(cu):
|
||||||
"""Cubochoric to homochoric"""
|
"""Cubochoric to homochoric"""
|
||||||
return Lambert.CubeToBall(cu)
|
return Lambert.CubeToBall(cu)
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue