def test_from_matrix(self): """Tests the extraction of a rotation quaternion from an AffineMatrix3D""" result = Quaternion.from_matrix(rotate_x(90)) answer = Quaternion(0.7071067811865475, 0.0, 0.0, 0.7071067811865476) self.assertAlmostEqual(answer.x, result.x, delta=1e-10, msg="Extracting quaternion from AffineMatrix3D produced wrong result [X].") self.assertAlmostEqual(answer.y, result.y, delta=1e-10, msg="Extracting quaternion from AffineMatrix3D produced wrong result [Y].") self.assertAlmostEqual(answer.z, result.z, delta=1e-10, msg="Extracting quaternion from AffineMatrix3D produced wrong result [Z].") self.assertAlmostEqual(answer.s, result.s, delta=1e-10, msg="Extracting quaternion from AffineMatrix3D produced wrong result [S].") matrix = AffineMatrix3D(((0.9330127, 0.0669873, -0.3535534, 0.0), (0.0669873, 0.9330127, 0.3535534, 0.0), (0.3535534, -0.3535534, 0.8660254, 0.0), (0.0, 0.0, 0.0, 1.0))) result = Quaternion.from_matrix(matrix) answer = Quaternion(-0.1830127018922193, -0.1830127018922193, -0.0, 0.9659258262890683) self.assertAlmostEqual(answer.x, result.x, delta=1e-6, msg="Extracting quaternion from AffineMatrix3D produced wrong result [X].") self.assertAlmostEqual(answer.y, result.y, delta=1e-6, msg="Extracting quaternion from AffineMatrix3D produced wrong result [Y].") self.assertAlmostEqual(answer.z, result.z, delta=1e-6, msg="Extracting quaternion from AffineMatrix3D produced wrong result [Z].") self.assertAlmostEqual(answer.s, result.s, delta=1e-6, msg="Extracting quaternion from AffineMatrix3D produced wrong result [S].")
def test_norm(self): """Norm of a quaternion""" q = Quaternion(2, 3, 4, 1) self.assertAlmostEqual(q.norm(), 5.477225575051661, delta=1e-10, msg="Norm of a quaternion failed to produce the correct result.")
def test_negation(self): """Negation operation""" q = Quaternion(1, 2, 3, 4) q_result = Quaternion(-1, -2, -3, -4) self.assertTrue(-q == q_result, "Negation of a quaternion failed to produce the correct result.")
def test_inverse(self): """Inverse operation""" q = Quaternion(0, 1, 0, 1) q_result = q.inverse() self.assertAlmostEqual( q_result.x, 0, delta=1e-10, msg= 'Inverse of a quaternion failed to produce the correct result [X].' ) self.assertAlmostEqual( q_result.y, -0.5, delta=1e-10, msg= 'Inverse of a quaternion failed to produce the correct result [Y].' ) self.assertAlmostEqual( q_result.z, 0, delta=1e-10, msg= 'Inverse of a quaternion failed to produce the correct result [Z].' ) self.assertAlmostEqual( q_result.s, 0.5, delta=1e-10, msg= 'Inverse of a quaternion failed to produce the correct result [S].' )
def test_normalise(self): """Normalising a quaternion""" q = Quaternion(2, 3, 4, 1) q_result = q.normalise() self.assertAlmostEqual( q_result.x, 0.3651483716701107, delta=1e-10, msg='Norm of a quaternion failed to produce the correct result.') self.assertAlmostEqual( q_result.y, 0.5477225575051661, delta=1e-10, msg='Norm of a quaternion failed to produce the correct result.') self.assertAlmostEqual( q_result.z, 0.7302967433402214, delta=1e-10, msg='Norm of a quaternion failed to produce the correct result.') self.assertAlmostEqual( q_result.s, 0.18257418583505536, delta=1e-10, msg='Norm of a quaternion failed to produce the correct result.')
def test_from_axis_angle(self): """Test generation of a quaternion from axis angle specification""" result = Quaternion.from_axis_angle(Vector3D(1, 0, 0), 45) answer = Quaternion(0.3826834323650898, 0.0, 0.0, 0.9238795325112867) self.assertAlmostEqual(answer.x, result.x, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [X].") self.assertAlmostEqual(answer.y, result.y, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [Y].") self.assertAlmostEqual(answer.z, result.z, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [Z].") self.assertAlmostEqual(answer.s, result.s, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [S].") result = Quaternion.from_axis_angle(Vector3D(0.5, 0.5, 0), -30) answer = Quaternion(-0.1830127018922193, -0.1830127018922193, -0.0, 0.9659258262890683) self.assertAlmostEqual(answer.y, result.y, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [Y].") self.assertAlmostEqual(answer.z, result.z, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [Z].") self.assertAlmostEqual(answer.x, result.x, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [X].") self.assertAlmostEqual(answer.s, result.s, delta=1e-10, msg="Converting axis angle to quaternion produced wrong result [S].")
def test_conjugate(self): """Test conjugation operation""" q = Quaternion(2, -3, 0, 1) result = q.conjugate() answer = Quaternion(-2, 3, 0, 1) self.assertEqual( result.x, answer.x, msg= 'Conjugation of a Quaternion failed to produce the correct result [X].' ) self.assertEqual( result.y, answer.y, msg= 'Conjugation of a Quaternion failed to produce the correct result [Y].' ) self.assertEqual( result.z, answer.z, msg= 'Conjugation of a Quaternion failed to produce the correct result [Z].' )
def test_z(self): """Get/set z co-ordinate.""" q = Quaternion(2.5, 6.7, -4.6, 1.0) # get z attribute self.assertEqual(q.z, -4.6, 'Getting z attribute failed.') # set z attribute q.z = 157.3 self.assertEqual(q.z, 157.3, 'Setting z attribute failed.')
def test_y(self): """Get/set y co-ordinate.""" q = Quaternion(2.5, 6.7, -4.6, 1.0) # get y attribute self.assertEqual(q.y, 6.7, 'Getting y attribute failed.') # set y attribute q.y = -7.1 self.assertEqual(q.y, -7.1, 'Setting y attribute failed.')
def test_x(self): """Get/set x co-ordinate.""" q = Quaternion(2.5, 6.7, -4.6, 1.0) # get x attribute self.assertEqual(q.x, 2.5, 'Getting x attribute failed.') # set x attribute q.x = 10.0 self.assertEqual(q.x, 10.0, 'Setting x attribute failed.')
def test_s(self): """Get/set s co-ordinate.""" q = Quaternion(2.5, 6.7, -4.6, 1.0) # get x attribute self.assertEqual(q.s, 1.0, "Getting s attribute failed.") # set x attribute q.s = 10.0 self.assertEqual(q.s, 10.0, "Setting s attribute failed.")
def test_add(self): """Addition operator.""" q1 = Quaternion(2, 3, 4, 1) q2 = Quaternion(2, -3, 10, 0) q_theory = Quaternion(4, 0, 14, 1) q_result = q1 + q2 self.assertEqual(q_result.x, q_theory.x, "Addition of two quaternions failed [X].") self.assertEqual(q_result.y, q_theory.y, "Addition of two quaternions failed [Y].") self.assertEqual(q_result.z, q_theory.z, "Addition of two quaternions failed [Z].") self.assertEqual(q_result.s, q_theory.s, "Addition of two quaternions failed [S].")
def test_not_equal(self): """Inequality operator.""" self.assertFalse(Quaternion(1, 2, 3, 4) != Quaternion(1, 2, 3, 4), "Inequality operator returned true for equal quaternions.") self.assertTrue(Quaternion(5, 1, 3, 4) != Quaternion(1, 2, 3, 4), "Inequality operator returned false for quaternions with non-equal x components.") self.assertTrue(Quaternion(1, 5, 3, 4) != Quaternion(1, 2, 3, 4), "Inequality operator returned false for quaternions with non-equal y components.") self.assertTrue(Quaternion(1, 2, 5, 4) != Quaternion(1, 2, 3, 4), "Inequality operator returned false for quaternions with non-equal z components.") self.assertTrue(Quaternion(1, 2, 3, 5) != Quaternion(1, 2, 3, 4), "Inequality operator returned false for quaternions with non-equal s components.")
def test_subtract(self): """Subtraction operator.""" q1 = Quaternion(2, 3, 4, 1) q2 = Quaternion(2, -3, 10, 0) # desired result for subtraction of q1 and q2 q_theory = Quaternion(0, 6, -6, 1) q_result = q1 - q2 self.assertEqual(q_result.x, q_theory.x, "Subtraction of two quaternions failed [X].") self.assertEqual(q_result.y, q_theory.y, "Subtraction of two quaternions failed [Y].") self.assertEqual(q_result.z, q_theory.z, "Subtraction of two quaternions failed [Z].") self.assertEqual(q_result.s, q_theory.s, "Subtraction of two quaternions failed [S].")
def test_multiplication(self): """Multiplication operation""" q1 = Quaternion(0, 1, 0, 1) q2 = Quaternion(0.5, 0.5, 0.75, 1) q_result = Quaternion(0, 3, 0, 3) self.assertTrue(q1 * 3 == q_result, "Multiplication of a quaternion and a scalar failed to produce the correct result.") self.assertTrue(3 * q1 == q_result, "Multiplication of a quaternion and a scalar failed to produce the correct result.") q_result = Quaternion(1.25, 1.5, 0.25, 0.5) self.assertTrue(q1 * q2 == q_result, "Multiplication of two quaternions failed to produce the correct result.")
def test_transform(self): # transform between space 1 and space 2 m = rotate_z(47) # define in coordinate space 1 v1 = Vector3D(1, 0.3, -0.2) q1 = Quaternion.from_axis_angle(Vector3D(0.1, -0.7, 0.2), 57) # transform to coordinate space 2 v2 = v1.transform(m) q2 = q1.transform(m) # use quaternion to rotate vector in each space r1 = v1.transform(q1.as_matrix()) r2 = v2.transform(q2.as_matrix()) # convert result in space 2 to space 1 for comparison r2_1 = r2.transform(m.inverse()) self.assertAlmostEqual(r1.x, r2_1.x, delta=1e-10, msg='Transform failed [X]') self.assertAlmostEqual(r1.y, r2_1.y, delta=1e-10, msg='Transform failed [Y]') self.assertAlmostEqual(r1.z, r2_1.z, delta=1e-10, msg='Transform failed [Z]')
def test_indexing(self): """Getting/setting components by indexing.""" q = Quaternion(2.5, 6.7, -4.6, 1.0) q[0] = 2.0 q[1] = 7.0 q[2] = 10.0 q[3] = 1.5 # check getting/setting via valid indexes self.assertEqual(q[0], 2.0, 'Indexing failed [X].') self.assertEqual(q[1], 7.0, 'Indexing failed [Y].') self.assertEqual(q[2], 10.0, 'Indexing failed [Z].') self.assertEqual(q[3], 1.5, 'Indexing failed [S].') # check invalid indexes with self.assertRaises( IndexError, msg='Invalid positive index did not raise IndexError.'): r = q[4] with self.assertRaises( IndexError, msg='Invalid negative index did not raise IndexError.'): r = q[-1]
def test_initialise_default(self): """Default initialisation, identity quaternion.""" q = Quaternion() self.assertEqual(q.x, 0.0, "Default initialisation is not (<0,0,0>,1) [X].") self.assertEqual(q.y, 0.0, "Default initialisation is not (<0,0,0>,1) [Y].") self.assertEqual(q.z, 0.0, "Default initialisation is not (<0,0,0>,1) [Z].") self.assertEqual(q.s, 1.0, "Default initialisation is not (<0,0,0>,1) [S].")
def test_to_matrix(self): """Test AffineMatrix3D generation from a quaternion""" message = "Conversion of a Quaternion to AffineMatrix3D failed to produce the correct result." matrix = Quaternion(0.5, 0, 0, 0.5).to_matrix() answer = rotate_x(90) # TODO - replace this with a utility function e.g. _assert_matrix() self.assertAlmostEqual(matrix[0, 0], answer[0, 0], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[0, 1], answer[0, 1], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[0, 2], answer[0, 2], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[0, 3], answer[0, 3], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[1, 0], answer[1, 0], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[1, 1], answer[1, 1], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[1, 2], answer[1, 2], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[1, 3], answer[1, 3], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[2, 0], answer[2, 0], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[2, 1], answer[2, 1], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[2, 2], answer[2, 2], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[2, 3], answer[2, 3], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[3, 0], answer[3, 0], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[3, 1], answer[3, 1], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[3, 2], answer[3, 2], delta=1e-10, msg=message) self.assertAlmostEqual(matrix[3, 3], answer[3, 3], delta=1e-10, msg=message) # TODO - increase the resolution of this test by calculating quaternion more accurately matrix = Quaternion(0.3826834, 0, 0, 0.923879).to_matrix() answer = rotate_x(45) self.assertAlmostEqual(matrix[0, 0], answer[0, 0], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[0, 1], answer[0, 1], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[0, 2], answer[0, 2], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[0, 3], answer[0, 3], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[1, 0], answer[1, 0], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[1, 1], answer[1, 1], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[1, 2], answer[1, 2], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[1, 3], answer[1, 3], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[2, 0], answer[2, 0], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[2, 1], answer[2, 1], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[2, 2], answer[2, 2], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[2, 3], answer[2, 3], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[3, 0], answer[3, 0], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[3, 1], answer[3, 1], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[3, 2], answer[3, 2], delta=1e-6, msg=message) self.assertAlmostEqual(matrix[3, 3], answer[3, 3], delta=1e-6, msg=message)
def test_initialise_indexable(self): """Initialisation with an indexable object.""" q = Quaternion(1.0, 2.0, 3.0, 4.0) self.assertEqual(q.x, 1.0, 'Initialisation with indexable failed [X].') self.assertEqual(q.y, 2.0, 'Initialisation with indexable failed [Y].') self.assertEqual(q.z, 3.0, 'Initialisation with indexable failed [Z].') self.assertEqual(q.s, 4.0, 'Initialisation with indexable failed [S].')
def test_division(self): """Division operation""" q1 = Quaternion(0, 1, 0, 1) q2 = Quaternion(0.5, 0.5, 0.75, 1) q_theory = Quaternion(-0.60606060606, 0.242424242424, -0.12121212121212, 0.7272727272) q_result = q1 / q2 self.assertAlmostEqual( q_result.x, q_theory.x, delta=1e-10, msg= 'Division of two quaternions failed to produce the correct result [X].' ) self.assertAlmostEqual( q_result.y, q_theory.y, delta=1e-10, msg= 'Division of two quaternions failed to produce the correct result [Y].' ) self.assertAlmostEqual( q_result.z, q_theory.z, delta=1e-10, msg= 'Division of two quaternions failed to produce the correct result [Z].' ) self.assertAlmostEqual( q_result.s, q_theory.s, delta=1e-10, msg= 'Division of two quaternions failed to produce the correct result [S].' ) q_result = Quaternion(0.25, 0.25, 0.375, 0.5) self.assertTrue( q2 / 2 == q_result, 'Division of a quaternion and a scalar failed to produce the correct result.' )
def test_iter(self): """Obtain values by iteration.""" q = Quaternion(2.5, 6.7, -4.6, 1.0) l = list(q) self.assertEqual(len(l), 4, "Iteration failed to return the correct number of items.") self.assertEqual(l[0], 2.5, "Iteration failed [X].") self.assertEqual(l[1], 6.7, "Iteration failed [Y].") self.assertEqual(l[2], -4.6, "Iteration failed [Z].") self.assertEqual(l[3], 1.0, "Iteration failed [S].")
def test_length(self): """Length (norm) of a quaternion""" q = Quaternion(2, 3, 4, 1) self.assertAlmostEqual( q.length, 5.477225575051661, delta=1e-10, msg='Length of a quaternion failed to produce the correct result.')
def test_is_unit(self): """Testing method is_unit()""" self.assertTrue( Quaternion(1, 0, 0, 0).is_unit(), 'Quaternion is_unit() test failed to produce the correct result.') self.assertTrue( Quaternion(0, 1, 0, 0).is_unit(), 'Quaternion is_unit() test failed to produce the correct result.') self.assertTrue( Quaternion(0, 0, 1, 0).is_unit(), 'Quaternion is_unit() test failed to produce the correct result.') self.assertTrue( Quaternion(0, 0, 0, 1).is_unit(), 'Quaternion is_unit() test failed to produce the correct result.') self.assertFalse( Quaternion(1, 2, 3, 4).is_unit(), 'Quaternion is_unit() test failed to produce the correct result.') self.assertFalse( Quaternion(-0.777777, 3.1214352, -0.9483527, 3.1415).is_unit(), 'Quaternion is_unit() test failed to produce the correct result.')
def test_quaternion_to(self): """Tests the calculation of a quaternion between two quaternions.""" q1 = Quaternion.from_axis_angle(Vector3D(1.0, 0.3, -0.1), 10) q2 = Quaternion.from_axis_angle(Vector3D(0.2, 1.0, 1.0), 25) d = q1.quaternion_to(q2) # transform a vector with q1 + d and q2, both should result in the same answer v = Vector3D(1, 3, 4) r1 = v.transform(q1.as_matrix()).transform(d.as_matrix()) r2 = v.transform(q2.as_matrix()) self.assertAlmostEqual(r1.x, r2.x, delta=1e-10, msg='Rotation_to failed [X]') self.assertAlmostEqual(r1.y, r2.y, delta=1e-10, msg='Rotation_to failed [Y]') self.assertAlmostEqual(r1.z, r2.z, delta=1e-10, msg='Rotation_to failed [Z]')
def test_copy(self): """Testing method copy()""" q = Quaternion(1, 2, 3, 4) r = q.copy() # check a new instance has been created by modifying the original q.x = 5.0 q.y = 6.0 q.z = 7.0 q.s = 8.0 self.assertEqual(r.x, 1.0, 'Copy failed [X].') self.assertEqual(r.y, 2.0, 'Copy failed [Y].') self.assertEqual(r.z, 3.0, 'Copy failed [Z].') self.assertEqual(r.s, 4.0, 'Copy failed [S].')
def test_initialise_invalid(self): """Initialisation with invalid types should raise a TypeError.""" with self.assertRaises(TypeError, msg='Initialised with a string.'): Quaternion('spoon')
def test_axis_angle_decomposition(self): q = Quaternion(1, 1, 1, 1) axis = q.axis angle = q.angle self.assertAlmostEqual( axis.x, 0.5773502691896258, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( axis.y, 0.5773502691896258, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( axis.z, 0.5773502691896258, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( angle, 120, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') q = Quaternion(2, 3, 4, 1) axis = q.axis angle = q.angle self.assertAlmostEqual( axis.x, 0.3713906763541038, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( axis.y, 0.5570860145311557, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( axis.z, 0.7427813527082076, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( angle, 158.96053021868278, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') q = Quaternion(0, 0, 0, 1) axis = q.axis angle = q.angle self.assertAlmostEqual( axis.x, 0, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( axis.y, 0, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( axis.z, 0, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.') self.assertAlmostEqual( angle, 0, delta=1e-10, msg='Decomposition of a quaternion into axis angle representation ' 'failed to produce the correct result.')