def test_inverse_equivalence(self):
q = [5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17]
result = matrix33.create_from_quaternion(quaternion.inverse(q))
expected = matrix33.inverse(matrix33.create_from_quaternion(q))
np.testing.assert_almost_equal(result, expected, decimal=5)
q = quaternion.create_from_x_rotation(0.5)
result = matrix33.create_from_inverse_of_quaternion(q)
expected = matrix33.inverse(matrix33.create_from_quaternion(q))
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_create_from_quaternion_rotation(self):
result = matrix33.create_from_quaternion([.57735, .57735, .57735, 0.])
expected = [
[-0.333333, 0.666667, 0.666667],
[0.666667, -0.333333, 0.666667],
[0.666667, 0.666667, -0.333333],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def test_euler_equivalence(self):
eulers = euler.create_from_x_rotation(np.pi / 2.)
m = matrix33.create_from_x_rotation(np.pi / 2.)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix33.create_from_quaternion(q)
em = matrix33.create_from_eulers(eulers)
self.assertTrue(np.allclose(qm, m))
self.assertTrue(np.allclose(qm, em))
self.assertTrue(np.allclose(m, em))
def test_create_from_axis_rotation_non_normalised(self):
result = matrix33.create_from_axis_rotation([1., 1., 1.], np.pi)
np.testing.assert_almost_equal(result,
matrix33.create_from_quaternion([
5.77350000e-01, 5.77350000e-01,
5.77350000e-01, 6.12323400e-17
]),
decimal=3)
self.assertTrue(result.dtype == np.float)
def test_create_from_quaternion_z(self):
result = matrix33.create_from_quaternion([0., 0., 1., 0.])
expected = [
[-1., 0., 0.],
[0., -1., 0.],
[0., 0., 1.],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def translate_backward(self, amount):
"""Translates the object backward along the inertial X,Z
plane.
"""
quat = quaternion.set_to_rotation_about_y(self.yaw)
matrix = matrix33.create_from_quaternion(quat)
vec = matrix33.apply_to_vector([0.0, 0.0, 1.0], matrix)
vec *= amount
self.transform.inertial.translate(vec)
def test_create_from_axis_rotation(self):
# wolfram alpha can be awesome sometimes
result = matrix33.create_from_axis_rotation(
[0.57735, 0.57735, 0.57735], np.pi)
np.testing.assert_almost_equal(result,
matrix33.create_from_quaternion([
5.77350000e-01, 5.77350000e-01,
5.77350000e-01, 6.12323400e-17
]),
decimal=3)
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 y(self):
"""Returns the object's local Y axis.
This is the Y axis rotated by the objects orientation.
.. note::
This is NOT the world orientation.
To get inertial Y axis, simply use [0.0, 1.0, 0.0].
"""
# convert our quaternion to a matrix
matrix = matrix33.create_from_quaternion(self.transform.orientation)
# apply the matrix to a Y vector
return matrix33.apply_to_vector(matrix, vector3.unit.y)
def test_operators_quaternion(self):
m = Matrix33.identity()
q = Quaternion.from_x_rotation(0.7)
# add
self.assertRaises(ValueError, lambda: m + q)
# subtract
self.assertRaises(ValueError, lambda: m - q)
# multiply
self.assertTrue(np.array_equal(m * q, matrix33.multiply(matrix33.create_from_quaternion(quaternion.create_from_x_rotation(0.7)), matrix33.create_identity())))
# divide
self.assertRaises(ValueError, lambda: m / q)
def translate(self, vector):
"""Translates the transform locally.
The vector will have the node's current orientation
applied to it and then be added to the translation.
"""
if numpy.array_equal(vector, [0.0, 0.0, 0.0]):
# don't bother to update anything
return
# multiply the vector by our local orientation
# convert our quaternion to a matrix
matrix = matrix33.create_from_quaternion(self.transform._orientation)
# apply the matrix to the specified vector
self.transform.translation += matrix33.apply_to_vector(
matrix, numpy.array(vector))
def test_create_from_quaternion_equivalent(self):
result = matrix33.create_from_quaternion(
quaternion.create_from_x_rotation(0.5))
expected = matrix33.create_from_x_rotation(0.5)
np.testing.assert_almost_equal(result, expected, decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_inverse_quaternion(self):
q = Quaternion.from_x_rotation(0.5)
m = Matrix33.from_inverse_of_quaternion(q)
expected = matrix33.create_from_quaternion(quaternion.inverse(quaternion.create_from_x_rotation(0.5)))
np.testing.assert_almost_equal(np.array(m), expected, decimal=5)
def test_matrix33(self):
q = Quaternion.from_x_rotation(np.pi / 2.0)
self.assertTrue(
np.allclose(q.matrix33, matrix33.create_from_quaternion(q)))
def test_create_from_quaternion_unit(self):
result = matrix33.create_from_quaternion([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, np.eye(3), decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create_from_quaternion_rotated_z(self):
quat = quaternion.create_from_z_rotation(np.pi)
result = matrix33.create_from_quaternion(quat)
expected = matrix33.create_from_z_rotation(np.pi)
self.assertTrue(np.allclose(result, expected))
def test_create_from_quaternion_identity(self):
quat = quaternion.create()
result = matrix33.create_from_quaternion(quat)
expected = np.eye(3)
self.assertTrue(np.array_equal(result, expected))
