polishing
This commit is contained in:
parent
326b82b3aa
commit
daab5a8952
|
@ -130,21 +130,21 @@ def qu2ho(qu):
|
||||||
|
|
||||||
|
|
||||||
#---------- Rotation matrix ----------
|
#---------- Rotation matrix ----------
|
||||||
def om2qu(a):
|
def om2qu(om):
|
||||||
trace = a[0,0] + a[1,1] + a[2,2]
|
trace = om.trace()
|
||||||
if trace > 0:
|
if trace > 0:
|
||||||
s = 0.5 / np.sqrt(trace+ 1.0)
|
s = 0.5 / np.sqrt(trace+ 1.0)
|
||||||
qu = np.array([0.25 / s,( a[2,1] - a[1,2] ) * s,( a[0,2] - a[2,0] ) * s,( a[1,0] - a[0,1] ) * s])
|
qu = np.array([0.25 / s,( om[2,1] - om[1,2] ) * s,( om[0,2] - om[2,0] ) * s,( om[1,0] - om[0,1] ) * s])
|
||||||
else:
|
else:
|
||||||
if ( a[0,0] > a[1,1] and a[0,0] > a[2,2] ):
|
if ( om[0,0] > om[1,1] and om[0,0] > om[2,2] ):
|
||||||
s = 2.0 * np.sqrt( 1.0 + a[0,0] - a[1,1] - a[2,2])
|
s = 2.0 * np.sqrt( 1.0 + om[0,0] - om[1,1] - om[2,2])
|
||||||
qu = np.array([ (a[2,1] - a[1,2]) / s,0.25 * s,(a[0,1] + a[1,0]) / s,(a[0,2] + a[2,0]) / s])
|
qu = np.array([ (om[2,1] - om[1,2]) / s,0.25 * s,(om[0,1] + om[1,0]) / s,(om[0,2] + om[2,0]) / s])
|
||||||
elif (a[1,1] > a[2,2]):
|
elif (om[1,1] > om[2,2]):
|
||||||
s = 2.0 * np.sqrt( 1.0 + a[1,1] - a[0,0] - a[2,2])
|
s = 2.0 * np.sqrt( 1.0 + om[1,1] - om[0,0] - om[2,2])
|
||||||
qu = np.array([ (a[0,2] - a[2,0]) / s,(a[0,1] + a[1,0]) / s,0.25 * s,(a[1,2] + a[2,1]) / s])
|
qu = np.array([ (om[0,2] - om[2,0]) / s,(om[0,1] + om[1,0]) / s,0.25 * s,(om[1,2] + om[2,1]) / s])
|
||||||
else:
|
else:
|
||||||
s = 2.0 * np.sqrt( 1.0 + a[2,2] - a[0,0] - a[1,1] )
|
s = 2.0 * np.sqrt( 1.0 + om[2,2] - om[0,0] - om[1,1] )
|
||||||
qu = np.array([ (a[1,0] - a[0,1]) / s,(a[0,2] + a[2,0]) / s,(a[1,2] + a[2,1]) / s,0.25 * s])
|
qu = np.array([ (om[1,0] - om[0,1]) / s,(om[0,2] + om[2,0]) / s,(om[1,2] + om[2,1]) / s,0.25 * s])
|
||||||
if qu[0]<0: qu*=-1
|
if qu[0]<0: qu*=-1
|
||||||
return qu*np.array([1.,_P,_P,_P])
|
return qu*np.array([1.,_P,_P,_P])
|
||||||
|
|
||||||
|
@ -163,7 +163,6 @@ def om2eu(om):
|
||||||
|
|
||||||
def om2ax(om):
|
def om2ax(om):
|
||||||
"""Rotation matrix to axis angle pair."""
|
"""Rotation matrix to axis angle pair."""
|
||||||
#return qu2ax(om2qu(om)) # HOTFIX
|
|
||||||
ax=np.empty(4)
|
ax=np.empty(4)
|
||||||
|
|
||||||
# first get the rotation angle
|
# first get the rotation angle
|
||||||
|
@ -446,11 +445,6 @@ def mul(me, other):
|
||||||
other : numpy.ndarray or Rotation
|
other : numpy.ndarray or Rotation
|
||||||
Vector, second or fourth order tensor, or rotation object that is rotated.
|
Vector, second or fourth order tensor, or rotation object that is rotated.
|
||||||
|
|
||||||
Todo
|
|
||||||
----
|
|
||||||
Document details active/passive)
|
|
||||||
consider rotation of (3,3,3,3)-matrix
|
|
||||||
|
|
||||||
"""
|
"""
|
||||||
if me.quaternion.shape != (4,):
|
if me.quaternion.shape != (4,):
|
||||||
raise NotImplementedError('Support for multiple rotations missing')
|
raise NotImplementedError('Support for multiple rotations missing')
|
||||||
|
|
Loading…
Reference in New Issue