Exemplo n.º 1
0
 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)
Exemplo n.º 2
0
 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)
Exemplo n.º 3
0
 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)
Exemplo n.º 4
0
 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)
Exemplo n.º 5
0
 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)