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