def test_slerp(self):
transform1 = rotate_z(30.0)
q1 = Quaternion.from_transform(transform1)
transform2 = rotate_z(90.0)
q2 = Quaternion.from_transform(transform2)
q_interp = slerp(0.5, q1, q2)
transform3 = rotate_z(60)
q3 = Quaternion.from_transform(transform3)
self.assertEqual(q_interp, q3)
self.assertEqual(q_interp.to_transform(), transform3)
def decompose(matrix):
"""Decompose a Matrix4x4 into T, R and S components.
Returns:
T , Vector
R , Quaternion
S , Matrix4x4
"""
R = Quaternion()
S = Matrix4x4()
# Extract translation _T_ from transformation matrix
T = Vector(matrix.m[0][3],
matrix.m[1][3],
matrix.m[2][3])
# Compute new transformation matrix _M_ without translation
M = Matrix4x4.from_matrix4x4(matrix)
for i in range(3):
M.m[i][3] = 0.0
M.m[3][i] = 0.0
M.m[3][3] = 1.0
# Extract rotation _R_ from transformation matrix
norm = 1.0
count = 0
R = Matrix4x4.from_matrix4x4(M)
while (count<100 and norm>0.0001):
# Compute next matrix _Rnext_ in series
Rnext = Matrix4x4()
Rit = inverse(transpose(R))
for i in range(4):
for j in range(4):
Rnext.m[i][j] = 0.5 * (R.m[i][j] + Rit.m[i][j])
# Compute norm of difference between _R_ and _Rnext_
norm = 0.0
for i in range(3):
n = abs(R.m[i][0] - Rnext.m[i][0]) + \
abs(R.m[i][1] - Rnext.m[i][1]) + \
abs(R.m[i][2] - Rnext.m[i][2])
norm = max(norm, n)
R = Rnext
count += 1
# XXX TODO FIXME deal with flip...
Rquat = Quaternion.from_transform(Transform(R))
# Compute scale _S_ using rotation and original matrix
S = inverse(R) * M
return T, Rquat, S
def decompose(matrix):
"""Decompose a Matrix4x4 into T, R and S components.
Returns:
T , Vector
R , Quaternion
S , Matrix4x4
"""
R = Quaternion()
S = Matrix4x4()
# Extract translation _T_ from transformation matrix
T = Vector(matrix.m[0][3], matrix.m[1][3], matrix.m[2][3])
# Compute new transformation matrix _M_ without translation
M = Matrix4x4.from_matrix4x4(matrix)
for i in range(3):
M.m[i][3] = 0.0
M.m[3][i] = 0.0
M.m[3][3] = 1.0
# Extract rotation _R_ from transformation matrix
norm = 1.0
count = 0
R = Matrix4x4.from_matrix4x4(M)
while (count < 100 and norm > 0.0001):
# Compute next matrix _Rnext_ in series
Rnext = Matrix4x4()
Rit = inverse(transpose(R))
for i in range(4):
for j in range(4):
Rnext.m[i][j] = 0.5 * (R.m[i][j] + Rit.m[i][j])
# Compute norm of difference between _R_ and _Rnext_
norm = 0.0
for i in range(3):
n = abs(R.m[i][0] - Rnext.m[i][0]) + \
abs(R.m[i][1] - Rnext.m[i][1]) + \
abs(R.m[i][2] - Rnext.m[i][2])
norm = max(norm, n)
R = Rnext
count += 1
# XXX TODO FIXME deal with flip...
Rquat = Quaternion.from_transform(Transform(R))
# Compute scale _S_ using rotation and original matrix
S = inverse(R) * M
return T, Rquat, S
def test_normalize(self):
q = Quaternion.from_transform(rotate_z(30.0))
self.assertEqual(normalize(q), q)
def test_transform(self):
q3 = Quaternion.from_transform(self.q2.to_transform())
self.assertEqual(q3, self.q2)
