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 testQuaternionToMatrixRT(self):
"""Round-trip test quaternion to matrix and back"""
# Numbers pulled out of air
Qin = spc.quaternionNormalize(np.array([0.25, 0.5, 0.71, 0.25]))
matrix = spc.quaternionToMatrix(Qin)
Qrt = spc.quaternionFromMatrix(matrix)
if np.sign(Qrt[-1]) != np.sign(Qin[-1]):
Qrt *= -1 #Handle the sign ambiguity
np.testing.assert_array_almost_equal(Qrt, Qin)
def test_quaternionToMatrix_simple(self):
"""Test several simple rotations"""
# Rotations by 90 degrees around X, Y, and Z axis
cos45 = 0.5 ** 0.5 # 1/sqrt(2), or cos/sin of 45 degrees
inputs = np.array([
[cos45, 0, 0, cos45],
[0, cos45, 0, cos45],
[0, 0, cos45, cos45],
])
expected = np.array([
[[1, 0, 0], [0, 0, -1], [0, 1, 0]],
[[0, 0, 1], [0, 1, 0], [-1, 0, 0]],
[[0, -1, 0], [1, 0, 0], [0, 0, 1]],
])
actual = spc.quaternionToMatrix(inputs)
np.testing.assert_array_almost_equal(actual, expected)
# Put scalar on other side
inputs = np.array([
[cos45, cos45, 0, 0],
[cos45, 0, cos45, 0],
[cos45, 0, 0, cos45],
])
actual = spc.quaternionToMatrix(inputs, scalarPos='first')
np.testing.assert_array_almost_equal(actual, expected)
def test_quaternionToMatrix_errors(self):
"""Test bad input"""
# Rotation by 90 degrees around X axis
Qin = np.array([0.5**0.5, 0, 0, 0.5**0.5])
with self.assertRaises(NotImplementedError) as cm:
spc.quaternionToMatrix(Qin, 'FOO')
self.assertEqual(
'quaternionToMatrix: scalarPos must be set to "First" or "Last"',
str(cm.exception))
with self.assertRaises(ValueError) as cm:
spc.quaternionToMatrix([1, 2, 3])
self.assertEqual('Input does not appear to be quaternion, wrong size.',
str(cm.exception))
with self.assertRaises(ValueError) as cm:
spc.quaternionToMatrix([1, 2, 3, 4], normalize=False)
self.assertEqual('Input quaternion not normalized.', str(cm.exception))
actual = spc.quaternionToMatrix([1, 2, 3, 4])
expected = spc.quaternionToMatrix(spc.quaternionNormalize([1, 2, 3,
4]))
np.testing.assert_array_almost_equal(actual, expected)
def test_quaternionFromMatrix_rt(self):
"""Round-trip arbitrary rotation matrix to quaternion and back"""
# Same matrix as test_quaternionFromMatrix_nasty
u = [12. / 41, -24. / 41, 31. / 41]
theta = np.radians(58)
ux, uy, uz = u
c = np.cos(theta)
s = np.sin(theta)
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)
matrix_rt = spc.quaternionToMatrix(Qout)
np.testing.assert_array_almost_equal(
matrix_rt, matrix)