def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = matrix44.create_from_x_rotation(np.pi / 2.)
mq = quaternion.create_from_matrix(m)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix44.create_from_quaternion(q)
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(q, [1., 0., 0., 1.]),
[1., 0., 0., 1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(mq, [1., 0., 0., 1.]),
[1., 0., 0., 1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def test_create_from_matrix_x(self):
result = quaternion.create_from_matrix([
[1., 0., 0.],
[0., -1., 0.],
[0., 0., -1.],
])
np.testing.assert_almost_equal(result, [1., 0., 0., 0.], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_matrix_y(self):
result = quaternion.create_from_matrix([
[-1., 0., 0.],
[0., 1., 0.],
[0., 0., -1.],
])
np.testing.assert_almost_equal(result, [0., 1., 0., 0.], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = quaternion.create(0.7071, 0., 0., 0.7071)
m33 = matrix33.create_from_quaternion(q)
expected = np.array([
[1., 0., 0.],
[0.,-0.,-1.],
[0., 1.,-0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = quaternion.create(*[0.80087974, 0.03166748, 0.59114721,-0.09018753])
m33 = matrix33.create_from_quaternion(q)
q2 = quaternion.create_from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = quaternion.create_from_matrix(m33.T)
self.assertFalse(np.allclose(q, q3))
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = quaternion.create(0.7071, 0., 0., 0.7071)
m33 = matrix33.create_from_quaternion(q)
expected = np.array([
[1., 0., 0.],
[0., -0., -1.],
[0., 1., -0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = quaternion.create(
*[0.80087974, 0.03166748, 0.59114721, -0.09018753])
m33 = matrix33.create_from_quaternion(q)
q2 = quaternion.create_from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = quaternion.create_from_matrix(m33.T)
self.assertFalse(np.allclose(q, q3))
def test_operators_matrix44(self):
q = Quaternion()
m = Matrix44.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q + m)
# subtract
self.assertRaises(ValueError, lambda: q - m)
# multiply
self.assertTrue(np.array_equal(q * m, quaternion.cross(quaternion.create(), quaternion.create_from_matrix(matrix44.create_from_x_rotation(0.5)))))
# divide
self.assertRaises(ValueError, lambda: q / m)
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = matrix44.create_from_x_rotation(np.pi / 2.)
mq = quaternion.create_from_matrix(m)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix44.create_from_quaternion(q)
self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], m), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], qm), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(mq, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def test_create_from_matrix_unit(self):
result = quaternion.create_from_matrix(np.eye(3))
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_matrix_unit(self):
result = quaternion.create_from_matrix(np.eye(3))
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
self.assertTrue(result.dtype == np.float)
