def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = matrix44.create_from_x_rotation(np.pi / 2.)
mq = quaternion.create_from_matrix(m)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix44.create_from_quaternion(q)
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(q, [1., 0., 0., 1.]),
[1., 0., 0., 1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(mq, [1., 0., 0., 1.]),
[1., 0., 0., 1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def __init__(self, data):
self.children = data.get('children')
self.matrix = data.get('matrix')
self.mesh = data.get('mesh')
self.camera = data.get('camera')
self.translation = data.get('translation')
self.rotation = data.get('rotation')
self.scale = data.get('scale')
if self.matrix is None:
self.matrix = matrix44.create_identity()
if self.translation is not None:
self.matrix = matrix44.create_from_translation(self.translation)
if self.rotation is not None:
quat = quaternion.create(self.rotation[0], self.rotation[1],
self.rotation[2], self.rotation[3])
mat = matrix44.create_from_quaternion(quat)
self.matrix = matrix44.multiply(mat, self.matrix)
if self.scale is not None:
self.matrix = matrix44.multiply(
matrix44.create_from_scale(self.scale), self.matrix)
def test_decompose(self):
# define expectations
expected_scale = vector3.create(*[1, 1, 2], dtype='f4')
expected_rotation = quaternion.create_from_y_rotation(np.pi, dtype='f4')
expected_translation = vector3.create(*[10, 0, -5], dtype='f4')
expected_model = np.array([
[-1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, -2, 0],
[10, 0, -5, 1],
], dtype='f4')
# compose matrix using Pyrr
s = matrix44.create_from_scale(expected_scale, dtype='f4')
r = matrix44.create_from_quaternion(expected_rotation, dtype='f4')
t = matrix44.create_from_translation(expected_translation, dtype='f4')
model = s.dot(r).dot(t)
np.testing.assert_almost_equal(model, expected_model)
self.assertTrue(model.dtype == expected_model.dtype)
# decompose matrix
scale, rotation, translation = matrix44.decompose(model)
np.testing.assert_almost_equal(scale, expected_scale)
self.assertTrue(scale.dtype == expected_scale.dtype)
np.testing.assert_almost_equal(rotation, expected_rotation)
self.assertTrue(rotation.dtype == expected_rotation.dtype)
np.testing.assert_almost_equal(translation, expected_translation)
self.assertTrue(translation.dtype == expected_translation.dtype)
def test_procedural_examples(self):
from pyrr import quaternion, matrix44, vector3
import numpy as np
point = vector3.create(1.,2.,3.)
orientation = quaternion.create()
translation = vector3.create()
scale = vector3.create(1,1,1)
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = quaternion.create_from_y_rotation(np.pi / 2.0)
orientation = quaternion.cross(rotation, orientation)
# create a matrix
# start our matrix off using the scale
matrix = matrix44.create_from_scale(scale)
# apply our orientation
orientation = matrix44.create_from_quaternion(orientation)
matrix = matrix44.multiply(matrix, orientation)
# apply our translation
translation_matrix = matrix44.create_from_translation(translation)
matrix = matrix44.multiply(matrix, translation_matrix)
# transform our point by the matrix
point = matrix44.apply_to_vector(matrix, point)
def matrix(self):
"""A matrix representing the transform's translation,
orientation and scale.
The is an @property decorated method which allows
retrieval and assignment of the scale value.
"""
if self._matrix == None:
# matrix transformations must be done in order
# scaling
# rotation
# translation
# apply our scale
self._matrix = matrix44.create_from_scale(self.scale)
# apply our quaternion
self._matrix = matrix44.multiply(
self._matrix,
matrix44.create_from_quaternion(self.orientation))
# apply our translation
# we MUST do this after the orientation
self._matrix = matrix44.multiply(
self._matrix,
matrix44.create_from_translation(self.translation))
return self._matrix
def test_procedural_examples(self):
from pyrr import quaternion, matrix44, vector3
import numpy as np
point = vector3.create(1.,2.,3.)
orientation = quaternion.create()
translation = vector3.create()
scale = vector3.create(1,1,1)
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = quaternion.create_from_y_rotation(np.pi / 2.0)
orientation = quaternion.cross(rotation, orientation)
# create a matrix
# start our matrix off using the scale
matrix = matrix44.create_from_scale(scale)
# apply our orientation
orientation = matrix44.create_from_quaternion(orientation)
matrix = matrix44.multiply(matrix, orientation)
# apply our translation
translation_matrix = matrix44.create_from_translation(translation)
matrix = matrix44.multiply(matrix, translation_matrix)
# transform our point by the matrix
point = matrix44.apply_to_vector(matrix, point)
def matrix( self ):
"""A matrix representing the transform's translation,
orientation and scale.
The is an @property decorated method which allows
retrieval and assignment of the scale value.
"""
if self._matrix == None:
# matrix transformations must be done in order
# scaling
# rotation
# translation
# apply our scale
self._matrix = matrix44.create_from_scale( self.scale )
# apply our quaternion
self._matrix = matrix44.multiply(
self._matrix,
matrix44.create_from_quaternion( self.orientation )
)
# apply our translation
# we MUST do this after the orientation
self._matrix = matrix44.multiply(
self._matrix,
matrix44.create_from_translation( self.translation )
)
return self._matrix
def test_create_from_axis_rotation_non_normalized(self):
result = matrix44.create_from_axis_rotation([1., 1., 1.], np.pi)
np.testing.assert_almost_equal(result,
matrix44.create_from_quaternion([
5.77350000e-01, 5.77350000e-01,
5.77350000e-01, 6.12323400e-17
]),
decimal=3)
self.assertTrue(result.dtype == np.float)
def test_create_from_quaternion_rotation(self):
result = matrix44.create_from_quaternion([.57735, .57735, .57735, 0.])
expected = [
[-0.333333, 0.666667, 0.666667, 0.],
[0.666667, -0.333333, 0.666667, 0.],
[0.666667, 0.666667, -0.333333, 0.],
[0., 0., 0., 1.],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_quaternion_z(self):
result = matrix44.create_from_quaternion([0.,0.,1.,0.])
expected = [
[-1.,0.,0.,0.],
[0.,-1.,0.,0.],
[0.,0.,1.,0.],
[0.,0.,0.,1.],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def rotated_z():
quat = quaternion.create_from_z_rotation( math.pi )
result = matrix44.create_from_quaternion( quat )
expected = matrix44.create_from_z_rotation( math.pi )
self.assertTrue(
numpy.allclose( result, expected ),
"Matrix44 from quaternion incorrect with PI rotation about Z"
)
def identity():
quat = quaternion.create_identity()
result = matrix44.create_from_quaternion( quat )
expected = numpy.eye( 4 )
self.assertTrue(
numpy.array_equal( result, expected ),
"Matrix44 from quaternion incorrect with identity quaternion"
)
def test_create_from_quaternion_rotation(self):
result = matrix44.create_from_quaternion([.57735,.57735,.57735,0.])
expected = [
[-0.333333, 0.666667, 0.666667,0.],
[0.666667, -0.333333, 0.666667,0.],
[0.666667, 0.666667, -0.333333,0.],
[0.,0.,0.,1.],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_quaternion_z(self):
result = matrix44.create_from_quaternion([0., 0., 1., 0.])
expected = [
[-1., 0., 0., 0.],
[0., -1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_axis_rotation(self):
# wolfram alpha can be awesome sometimes
result = matrix44.create_from_axis_rotation(
[0.57735, 0.57735, 0.57735], np.pi)
np.testing.assert_almost_equal(result,
matrix44.create_from_quaternion([
5.77350000e-01, 5.77350000e-01,
5.77350000e-01, 6.12323400e-17
]),
decimal=3)
self.assertTrue(result.dtype == np.float)
def update(self, x, z):
rotation_x = quaternion.Quaternion(
quaternion.create_from_eulers([x, 0, 0]))
rotation_z = self.rotation_x.conjugate.cross(
quaternion.Quaternion(quaternion.create_from_eulers([0, 0, z])))
self.rotation_x = self.rotation_x.cross(rotation_x)
self.matrix = numpy.matmul(
self.matrix,
matrix44.create_from_quaternion(rotation_z.cross(self.rotation_x)))
self.inverse_matrix = numpy.linalg.inv(self.matrix)
def test_operators_quaternion(self):
m = Matrix44.identity()
q = Quaternion.from_x_rotation(0.7)
# add
self.assertRaises(ValueError, lambda: m + q)
# subtract
self.assertRaises(ValueError, lambda: m - q)
# multiply
self.assertTrue(np.array_equal(m * q, matrix44.multiply(matrix44.create_from_quaternion(quaternion.create_from_x_rotation(0.7)), matrix44.create_identity())))
# divide
self.assertRaises(ValueError, lambda: m / q)
def test_operators_quaternion(self):
m = Matrix44.identity()
q = Quaternion.from_x_rotation(0.7)
# add
self.assertRaises(ValueError, lambda: m + q)
# subtract
self.assertRaises(ValueError, lambda: m - q)
# multiply
self.assertTrue(np.array_equal(m * q, matrix44.multiply(matrix44.create_from_quaternion(quaternion.create_from_x_rotation(0.7)), matrix44.create_identity())))
# divide
self.assertRaises(ValueError, lambda: m / q)
def update_world_matrices(node, gltf, world_matrix=None):
if 'matrix' not in node:
matrix = matrix44.create_from_quaternion(np.array(node['rotation']))
matrix[:3, 0] *= node['scale'][0]
matrix[:3, 1] *= node['scale'][1]
matrix[:3, 2] *= node['scale'][2]
matrix[:3, 3] = node['translation']
else:
matrix = np.array(node['matrix'], dtype=np.float32).reshape((4, 4)).T
if world_matrix is None:
world_matrix = matrix
else:
world_matrix = world_matrix.dot(matrix)
node['world_matrix'] = world_matrix.T
for child in [gltf['nodes'][n] for n in node['children']]:
update_world_matrices(child, gltf, world_matrix=world_matrix)
def update_world_matrices(node, gltf, world_matrix=None):
if 'matrix' not in node:
matrix = matrix44.create_from_quaternion(np.array(node['rotation']))
matrix[:3, 0] *= node['scale'][0]
matrix[:3, 1] *= node['scale'][1]
matrix[:3, 2] *= node['scale'][2]
matrix[:3, 3] = node['translation']
else:
matrix = np.array(node['matrix'], dtype=np.float32).reshape((4, 4)).T
if world_matrix is None:
world_matrix = matrix
else:
world_matrix = world_matrix.dot(matrix)
node['world_matrix'] = world_matrix.T
for child in [gltf['nodes'][n] for n in node['children']]:
update_world_matrices(child, gltf, world_matrix=world_matrix)
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = matrix44.create_from_x_rotation(np.pi / 2.)
mq = quaternion.create_from_matrix(m)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix44.create_from_quaternion(q)
self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], m), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], qm), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(mq, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def test_create_from_axis_rotation_non_normalised(self):
result = matrix44.create_from_axis_rotation([1.,1.,1.], np.pi)
np.testing.assert_almost_equal(result, matrix44.create_from_quaternion([5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17]), decimal=3)
self.assertTrue(result.dtype == np.float)
def test_create_from_axis_rotation(self):
# wolfram alpha can be awesome sometimes
result = matrix44.create_from_axis_rotation([0.57735, 0.57735, 0.57735],np.pi)
np.testing.assert_almost_equal(result, matrix44.create_from_quaternion([5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17]), decimal=3)
self.assertTrue(result.dtype == np.float)
def test_create_from_inverse_quaternion(self):
q = Quaternion.from_x_rotation(0.5)
m = Matrix44.from_inverse_of_quaternion(q)
expected = matrix44.create_from_quaternion(quaternion.inverse(quaternion.create_from_x_rotation(0.5)))
np.testing.assert_almost_equal(np.array(m), expected, decimal=5)
def test_create_from_quaternion_unit(self):
result = matrix44.create_from_quaternion([0.,0.,0.,1.])
np.testing.assert_almost_equal(result, np.eye(4), decimal=5)
self.assertTrue(result.dtype == np.float)
def _CreateMatrix(self): self.worldMatrix = matrix44.multiply( matrix44.create_from_translation(self.position), matrix44.create_from_quaternion(self.rotation) )
def test_create_from_quaternion_unit(self):
result = matrix44.create_from_quaternion([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, np.eye(4), decimal=5)
self.assertTrue(result.dtype == np.float)
def test_matrix44(self):
q = Quaternion.from_x_rotation(np.pi / 2.0)
self.assertTrue(
np.allclose(q.matrix44, matrix44.create_from_quaternion(q)))
def generate_joint_matrix(joint):
# convert joint position and orientation to a matrix
matrix = matrix44.multiply(
matrix44.create_from_quaternion(joint.orientation),
matrix44.create_from_translation(joint.position))
return matrix44.inverse(matrix)
def test_create_from_inverse_quaternion(self):
q = Quaternion.from_x_rotation(0.5)
m = Matrix44.from_inverse_of_quaternion(q)
expected = matrix44.create_from_quaternion(
quaternion.inverse(quaternion.create_from_x_rotation(0.5)))
np.testing.assert_almost_equal(np.array(m), expected, decimal=5)
def test_matrix44(self):
q = Quaternion.from_x_rotation(np.pi / 2.0)
self.assertTrue(np.allclose(q.matrix44, matrix44.create_from_quaternion(q)))
