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_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_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]')