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_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_rotate(self):
"""Rotation by yaw, pitch and roll factory function."""
m = rotate(63, -40, 12)
r = rotate_y(-63) * rotate_x(40) * rotate_z(12)
for i in range(0, 4):
for j in range(0, 4):
self.assertAlmostEqual(m[i, j], r[i, j], places=14, msg="Rotate matrix generation failed (R"+str(i)+", C"+str(j)+").")
def test_rotate_x(self):
"""Rotation about x-axis matrix factory function."""
m = rotate_x(67)
a = pi * 67 / 180
r = [[1, 0, 0, 0],
[0, cos(a), -sin(a), 0],
[0, sin(a), cos(a), 0],
[0, 0, 0, 1]]
for i, row in enumerate(r):
for j, v in enumerate(row):
self.assertAlmostEqual(m[i, j], v, places=14, msg="Rotate_x matrix generation failed (R"+str(i)+", C"+str(j)+").")
