def test_quaternionToMatrix_nasty(self):
"""Pick an arbitrary rotation and verify it works"""
# Numbers pulled out of air
Qin = spc.quaternionNormalize(np.array([0.25, 0.5, 0.71, 0.25]))
matrix = spc.quaternionToMatrix(Qin)
# Verify it's a rotation matrix
ortho_test = np.dot(matrix, matrix.transpose())
np.testing.assert_array_almost_equal(
ortho_test, np.identity(3))
det = np.linalg.det(matrix)
self.assertTrue(det > 0) # Proper?
invect = np.array([[5, 3, 2], [1, 0, 0],
[0.2, 5, 20], [0, 2, 2]])
# Test matrix vs. quaternion rotation, single vector
expected = spc.quaternionRotateVector(Qin, invect[1, :])
actual = np.dot(matrix, invect[1, :])
np.testing.assert_array_almost_equal(
actual, expected)
# All vectors at once
expected = spc.quaternionRotateVector(
np.tile(Qin, (4, 1)), invect)
# Transform the row vectors into column vectors so the
# numpy multiplication gives the right result (then
# transform back to row vectors for comparison.)
actual = np.dot(matrix, invect.transpose()).transpose()
np.testing.assert_array_almost_equal(
actual, expected)
def test_quaternionFromMatrix_nasty(self):
"""Pick an arbitrary rotation and verify it works"""
# https://csm.mech.utah.edu/content/wp-content/uploads/2011/08/orthList.pdf
# Axis of rotation
u = [12. / 41, -24. / 41, 31. / 41]
# Rotation angle
theta = np.radians(58)
ux, uy, uz = u
c = np.cos(theta)
s = np.sin(theta)
# Construct rotation matrix from axis and angle
# This might be doable more nicely in matrix notation...
matrix = np.array([
[c + ux ** 2 * (1 - c),
ux * uy * (1 - c) - uz * s,
ux * uz * (1 - c) + uy * s],
[uy * ux * (1 - c) + uz * s,
c + uy ** 2 * (1 - c),
uy * uz * (1 - c) - ux * s],
[uz * ux * (1 - c) - uy * s,
uz * uy * (1 - c) + ux * s,
c + uz ** 2 * (1 - c)]
])
Qout = spc.quaternionFromMatrix(matrix)
# Sample inputs to rotate
invect = np.array([[5, 3, 2], [1, 0, 0], [.2, 5, 20],
[0, 2, 2]])
# Transform the row vectors into column vectors so the
# numpy multiplication gives the right result (then
# transform back to row vectors for comparison.)
expected = np.dot(matrix, invect.transpose()).transpose()
actual = spc.quaternionRotateVector(
np.tile(Qout, (4, 1)), invect)
np.testing.assert_array_almost_equal(
actual, expected)
def test_quaternionRotateVector(self):
"""Simple vector rotations"""
cos45 = 0.5**0.5 # 1/sqrt(2), or cos/sin of 45 degrees
# No rotation, 90 degrees around each of X, Y, and Z
Qin = np.array([
[0, 0, 0, 1],
[cos45, 0, 0, cos45],
[0, cos45, 0, cos45],
[0, 0, cos45, cos45],
])
invect = np.array([[1, 0, 0], [0, 0, 1], [1, 0, 0], [0, 1, 0]])
expected = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1], [-1, 0, 0]])
outvect = spc.quaternionRotateVector(Qin, invect)
np.testing.assert_array_almost_equal(outvect, expected)