def test_quaternionNormalize_small(self):
"""test quaternionNormalize for very small values"""
tst1 = spc.quaternionNormalize([1e-15, 0, 1e-15, 0], scalarPos='first')
tst2 = spc.quaternionNormalize([1e-15, 0, 1e-15, 0], scalarPos='last')
ans1 = [1.0, 0.0, 0.0, 0.0]
ans2 = [0.0, 0.0, 0.0, 1.0]
np.testing.assert_array_almost_equal(ans1, tst1)
np.testing.assert_array_almost_equal(ans2, tst2)
def test_quaternionNormalize_2(self):
"""quaternionNormalize should have known result and magnitude 1"""
tst1 = spc.quaternionNormalize([1, 0, 1, 0], scalarPos='first')
tst2 = spc.quaternionNormalize([1, 0, 1, 0], scalarPos='last')
ans = [0.70710678, 0.0, 0.70710678, 0]
np.testing.assert_array_almost_equal(ans, tst1)
np.testing.assert_array_almost_equal(ans, tst2)
np.testing.assert_almost_equal(1.0, np.linalg.norm(tst1))
np.testing.assert_almost_equal(1.0, np.linalg.norm(tst2))
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_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_quaternionNormalize_leadingdim(self):
"""test quaternionNormalize with leading degenerate dimensions"""
tst = spc.quaternionNormalize([[1, 0, 1, 0]])
ans = np.array([[0.7071068, 0, 0.7071068, 0]])
np.testing.assert_array_almost_equal(ans, tst)
def test_quaternionNormalize(self):
"""quaternionNormalize should have known results"""
tst = spc.quaternionNormalize([0.707, 0, 0.707, 0.2])
ans = [ 0.69337122, 0. , 0.69337122, 0.19614462]
np.testing.assert_array_almost_equal(ans, tst)