def _interpolateMat(mat, percent):
""" called only by interpolate3DTransform()
"""
if mat.shape != (16, ) and mat.shape != (4, 4):
raise ValueError("matrix should be of shape (4,4) or (16,)")
if mat.shape == (16, ):
mat = numpy.reshape(mat, (4, 4))
# if percent <0.0:
# raise ValueError('The parameter percent should be a positive float"')
# return
p = percent
transf = mat[:, :]
rotMat = numpy.identity(4, 'f')
rotMat[:3, :3] = transf.astype(numpy.float32)[:3, :3]
from mglutil.math.rotax import mat_to_quat
quat = mat_to_quat(matrix=numpy.array(rotMat).ravel())
angle = quat[3] * p
newRotMat = rotax([0., 0., 0.], quat[:3], angle * degtorad, transpose=1)
newTranMat = numpy.identity(4, 'f')
newTranMat[3][0] = transf[3][0] * p
newTranMat[3][1] = transf[3][1] * p
newTranMat[3][2] = transf[3][2] * p
transform = newRotMat
transform[3][0] = newTranMat[3][0]
transform[3][1] = newTranMat[3][1]
transform[3][2] = newTranMat[3][2]
# That's it.. the self.transform is now updated.
return transform
def mat_to_axis_angle(matrix):
"""
NOTE: This function is added by Yong 2/01/04: given a 4x4 transformation
matrix of hinge motion, now returns the rotation angle and axis (defined by
vector and a point) Please be noticed that if the motion is not hinge, the
function will complain and return none
"""
if matrix.shape != (16, ) and matrix.shape != (4, 4):
raise ValueError("matrix should be of shape (4,4) or (16,)")
return None
if matrix.shape == (16, ):
matrix = N.reshape(matrix, (4, 4))
from math import sin, cos, pi, sqrt, fabs, acos
degtorad = pi / 180.
transf = matrix
from mglutil.math.rotax import mat_to_quat
rotMat = N.identity(4, 'f')
rotMat[:3, :3] = transf[:3, :3]
qB = mat_to_quat(matrix=N.array(rotMat).ravel())
angle = qB[3]
sa = sin(angle * degtorad / 2.0)
if (fabs(angle) < 0.0005):
sa = 1
if angle > 180.:
vector = [-qB[0] / sa, -qB[1] / sa, -qB[2] / sa]
else:
vector = [qB[0] / sa, qB[1] / sa, qB[2] / sa]
tranMat = transf[3, :3]
# check if the transformation is a hinge motion
a = vector
b = tranMat
c = [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
a2 = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]
b2 = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]
c2 = c[0] * c[0] + c[1] * c[1] + c[2] * c[2]
theta = acos((c2 - a2 - b2) / (2 * sqrt(a2 * b2))) / pi * 180
if fabs(theta - 90) > 1e-4:
raise ValueError("The given transformation is not a hinge motion")
return None, None, None
ratio = sqrt(1. / (2. * (1. - cos(degtorad * angle))))
p = [tranMat[0] * ratio, tranMat[1] * ratio, tranMat[2] * ratio]
ang = 90. - angle / 2.0
rot = rotax([0., 0., 0.], vector, ang * degtorad, transpose=1)
rot = rot[:3, :3]
point = N.dot(p, rot)
return vector, point, angle
def mat_to_axis_angle( matrix ):
"""
NOTE: This function is added by Yong 2/01/04: given a 4x4 transformation
matrix of hinge motion, now returns the rotation angle and axis (defined by
vector and a point) Please be noticed that if the motion is not hinge, the
function will complain and return none
"""
if matrix.shape != (16,) and matrix.shape != (4,4):
raise ValueError("matrix should be of shape (4,4) or (16,)")
return None
if matrix.shape == (16,):
matrix = N.reshape(matrix, (4,4))
from math import sin, cos, pi, sqrt, fabs, acos
degtorad = pi/180.
transf = matrix
from mglutil.math.rotax import mat_to_quat
rotMat = N.identity(4, 'f')
rotMat[:3,:3] = transf[:3,:3]
qB = mat_to_quat(matrix=N.array(rotMat).ravel())
angle = qB[3]
sa=sin(angle*degtorad/2.0)
if(fabs(angle) < 0.0005):
sa = 1
if angle > 180.:
vector=[-qB[0]/sa, -qB[1]/sa, -qB[2]/sa]
else:
vector=[qB[0]/sa, qB[1]/sa, qB[2]/sa]
tranMat = transf[3,:3]
# check if the transformation is a hinge motion
a=vector
b=tranMat
c =[a[0]-b[0], a[1]-b[1], a[2]-b[2]]
a2= a[0]*a[0] + a[1]*a[1] + a[2]*a[2]
b2= b[0]*b[0] + b[1]*b[1] + b[2]*b[2]
c2= c[0]*c[0] + c[1]*c[1] + c[2]*c[2]
theta = acos((c2-a2-b2)/(2* sqrt(a2*b2))) / pi * 180
if fabs(theta -90) > 1e-4:
raise ValueError("The given transformation is not a hinge motion")
return None, None, None
ratio = sqrt( 1. / (2. * (1.- cos(degtorad * angle ))))
p = [tranMat[0]*ratio, tranMat[1]*ratio, tranMat[2]*ratio]
ang = 90. - angle/2.0
rot = rotax([0.,0.,0.], vector, ang*degtorad, transpose=1)
rot = rot[:3,:3]
point = N.dot(p, rot)
return vector, point, angle
def getTransforms(self, matrix):
vrml2=[]
#mymatrix = matrix.__copy__()
#mymatrix = Numeric.reshape(mymatrix, (16,))
mymatrix = Numeric.reshape(matrix, (16,))
rot,trans,scale=self.Transformable.Decompose4x4(mymatrix)
r = rotax.mat_to_quat(rot)
r[3]=r[3]*math.pi/180.0 #convert to rad
r[0] = round(r[0],6)
r[1] = round(r[1],6)
r[2] = round(r[2],6)
r[3] = round(r[3],6)
vrml2.append(" translation "+`trans[0]`+" "+`trans[1]`+" "+\
`trans[2]`+"\n")
vrml2.append(" rotation "+`r[0]`+" "+`r[1]`+" "+\
`r[2]`+" "+`r[3]`+"\n")
vrml2.append(" scale "+`scale[0]`+" "+`scale[1]`+" "+\
`scale[2]`+"\n")
return vrml2
def getTransforms(self, matrix):
vrml2 = []
#mymatrix = matrix.__copy__()
#mymatrix = Numeric.reshape(mymatrix, (16,))
mymatrix = Numeric.reshape(matrix, (16, ))
rot, trans, scale = self.Transformable.Decompose4x4(mymatrix)
r = rotax.mat_to_quat(rot)
r[3] = r[3] * math.pi / 180.0 #convert to rad
r[0] = round(r[0], 6)
r[1] = round(r[1], 6)
r[2] = round(r[2], 6)
r[3] = round(r[3], 6)
vrml2.append(" translation "+`trans[0]`+" "+`trans[1]`+" "+\
`trans[2]`+"\n")
vrml2.append(" rotation "+`r[0]`+" "+`r[1]`+" "+\
`r[2]`+" "+`r[3]`+"\n")
vrml2.append(" scale "+`scale[0]`+" "+`scale[1]`+" "+\
`scale[2]`+"\n")
return vrml2
def _interpolateMat(mat, percent):
""" called only by interpolate3DTransform()
"""
if mat.shape != (16,) and mat.shape != (4,4):
raise ValueError("matrix should be of shape (4,4) or (16,)")
return None
if mat.shape == (16,):
mat = N.reshape(mat, (4,4))
# if percent <0.0:
# raise ValueError('The parameter percent should be a positive float"')
# return
p = percent
transf = mat[:,:]
rotMat = N.identity(4, 'f')
rotMat[:3,:3]=transf.astype(N.Float32)[:3,:3]
from mglutil.math.rotax import mat_to_quat
quat = mat_to_quat(matrix=N.array(rotMat).ravel())
angle = quat[3] * p
newRotMat = rotax([0.,0.,0.], quat[:3], angle*degtorad, transpose = 1)
newTranMat = N.identity(4, 'f')
newTranMat[3][0] = transf[3][0]*p
newTranMat[3][1] = transf[3][1]*p
newTranMat[3][2] = transf[3][2]*p
transform = newRotMat
transform[3][0] = newTranMat[3][0]
transform[3][1] = newTranMat[3][1]
transform[3][2] = newTranMat[3][2]
# That's it.. the self.transform is now updated.
return transform
