consistent ordering

- qu, om, eu, ax, ro, ho, cu
This commit is contained in:
Martin Diehl 2019-04-16 21:47:29 +02:00
parent 3a7408a213
commit d842902a76
1 changed files with 97 additions and 101 deletions

View File

@ -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)