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 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 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)))