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)