def __frommatrix( self, A ):
"""
Convert from rotation matrix to quaternion
"""
t = np.sum( np.diag(A) ) + 1.0
if( t > sys.float_info.epsilon ):
s = 0.5/sqrt(t)
w = 0.25 / s
x1 = ( A[2,1] - A[1,2] ) * s
x2 = ( A[0,2] - A[2,0] ) * s
x3 = ( A[1,0] - A[0,1] ) * s
else:
if ( A[0,0] > A[1,1] and A[0,0] > A[2,2] ):
s = 2.0 *sqrt( 1.0 + A[0,0] - A[1,1] - A[2,2] )
x1 = 0.25 * s
x2 = (A[0,1] + A[1,0]) / s
x3 = (A[0,2] + A[2,0]) / s
w = (A[2,1] - A[1,2]) / s
elif (A[1,1] > A[2,2]):
s = 2.0 * sqrt( 1.0 + A[1,1] - A[0,0] - A[2,2] )
x1 = (A[0,1] + A[1,0] ) / s
x2 = 0.25 * s
x3 = (A[1,2] + A[2,1] ) / s
w = (A[0,2] - A[2,0] ) / s
else:
s = 2.0 * sqrt( 1.0 + A[2,2] - A[0,0] - A[1,1] )
x1 = (A[0,2] + A[2,0]) / s
x2 = (A[1,2] + A[2,1]) / s
x3 = 0.25 * s
w = (A[1,0] - A[0,1]) / s
# assign
cgquat.__init__( self, w, x1, x2, x3 )
def __frommatrix(self, A):
"""
Convert from rotation matrix to quaternion
"""
t = np.sum(np.diag(A)) + 1.0
if (t > sys.float_info.epsilon):
s = 0.5 / sqrt(t)
w = 0.25 / s
x1 = (A[2, 1] - A[1, 2]) * s
x2 = (A[0, 2] - A[2, 0]) * s
x3 = (A[1, 0] - A[0, 1]) * s
else:
if (A[0, 0] > A[1, 1] and A[0, 0] > A[2, 2]):
s = 2.0 * sqrt(1.0 + A[0, 0] - A[1, 1] - A[2, 2])
x1 = 0.25 * s
x2 = (A[0, 1] + A[1, 0]) / s
x3 = (A[0, 2] + A[2, 0]) / s
w = (A[2, 1] - A[1, 2]) / s
elif (A[1, 1] > A[2, 2]):
s = 2.0 * sqrt(1.0 + A[1, 1] - A[0, 0] - A[2, 2])
x1 = (A[0, 1] + A[1, 0]) / s
x2 = 0.25 * s
x3 = (A[1, 2] + A[2, 1]) / s
w = (A[0, 2] - A[2, 0]) / s
else:
s = 2.0 * sqrt(1.0 + A[2, 2] - A[0, 0] - A[1, 1])
x1 = (A[0, 2] + A[2, 0]) / s
x2 = (A[1, 2] + A[2, 1]) / s
x3 = 0.25 * s
w = (A[1, 0] - A[0, 1]) / s
# assign
cgquat.__init__(self, w, x1, x2, x3)
def __init__( self , q=[1,0,0,0], A=None ):
if ( A != None ):
# case where we have provided a numpy array as a rotation matrix
# convert to mat3
A = mat3( [ A[0,0], A[0,1], A[0,2],
A[1,0], A[1,1], A[1,2],
A[2,0], A[2,1], A[2,2] ] )
# initialise the quaternion
cgquat.__init__( self, A )
else:
# default is the four elements
cgquat.__init__( self, q )
def __init__(self, q=[1, 0, 0, 0], A=None):
if (A != None):
# case where we have provided a numpy array as a rotation matrix
# convert to mat3
A = mat3([
A[0, 0], A[0, 1], A[0, 2], A[1, 0], A[1, 1], A[1, 2], A[2, 0],
A[2, 1], A[2, 2]
])
# initialise the quaternion
cgquat.__init__(self, A)
else:
# default is the four elements
cgquat.__init__(self, q)
