def test_operators_quaternion(self):
q1 = Quaternion()
q2 = Quaternion.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q1 + q2)
# subtract
# we had to add this to enable np.array_equal to work
# as it uses subtraction
#self.assertRaises(ValueError, lambda: q1 - q2)
# multiply
self.assertTrue(
np.array_equal(
q1 * q2,
quaternion.cross(quaternion.create(),
quaternion.create_from_x_rotation(0.5))))
# divide
self.assertRaises(ValueError, lambda: q1 / q2)
# or
self.assertTrue(
np.array_equal(
q1 | q2,
quaternion.dot(quaternion.create(),
quaternion.create_from_x_rotation(0.5))))
# inverse
self.assertTrue(
np.array_equal(
~q2,
quaternion.conjugate(quaternion.create_from_x_rotation(0.5))))
def test_operators_quaternion(self):
q1 = Quaternion()
q2 = Quaternion.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q1 + q2)
# subtract
# we had to add this to enable np.array_equal to work
# as it uses subtraction
#self.assertRaises(ValueError, lambda: q1 - q2)
# multiply
self.assertTrue(np.array_equal(q1 * q2, quaternion.cross(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# divide
self.assertRaises(ValueError, lambda: q1 / q2)
# or
self.assertTrue(np.array_equal(q1 | q2, quaternion.dot(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# inverse
self.assertTrue(np.array_equal(~q2, quaternion.conjugate(quaternion.create_from_x_rotation(0.5))))
# ==
self.assertTrue(Quaternion() == Quaternion())
self.assertFalse(Quaternion() == Quaternion([0., 0., 0., 0.]))
# !=
self.assertTrue(Quaternion() != Quaternion([1., 1., 1., 1.]))
self.assertFalse(Quaternion() != Quaternion())
def test_apply_to_vector_non_unit(self):
q = Quaternion.from_x_rotation(np.pi)
# zero length
v = Vector3([0., 0., 0.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [0., 0., 0.])))
# >1 length
v = Vector3([2., 0., 0.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [2., 0., 0.])))
v = Vector3([0., 2., 0.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [0., 2., 0.])))
v = Vector3([0., 0., 2.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [0., 0., 2.])))
def test_create_from_x_rotation(self):
# 180 degree turn around X axis
q = quaternion.create_from_x_rotation(np.pi)
self.assertTrue(np.allclose(q, [1., 0., 0., 0.]))
# 90 degree rotation around X axis
q = quaternion.create_from_x_rotation(np.pi / 2.)
self.assertTrue(np.allclose(q, [np.sqrt(0.5), 0., 0., np.sqrt(0.5)]))
# -90 degree rotation around X axis
q = quaternion.create_from_x_rotation(-np.pi / 2.)
self.assertTrue(np.allclose(q, [-np.sqrt(0.5), 0., 0., np.sqrt(0.5)]))
def test_create_from_x_rotation(self):
# 180 degree turn around X axis
q = quaternion.create_from_x_rotation(np.pi)
self.assertTrue(np.allclose(q, [1., 0., 0., 0.]))
# 90 degree rotation around X axis
q = quaternion.create_from_x_rotation(np.pi / 2.)
self.assertTrue(np.allclose(q, [np.sqrt(0.5), 0., 0., np.sqrt(0.5)]))
# -90 degree rotation around X axis
q = quaternion.create_from_x_rotation(-np.pi / 2.)
self.assertTrue(np.allclose(q, [-np.sqrt(0.5), 0., 0., np.sqrt(0.5)]))
def test_apply_to_vector_non_unit(self):
q = Quaternion.from_x_rotation(np.pi)
# zero length
v = Vector3([0., 0., 0.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [0., 0., 0.])))
# >1 length
v = Vector3([2., 0., 0.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [2., 0., 0.])))
v = Vector3([0., 2., 0.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [0., 2., 0.])))
v = Vector3([0., 0., 2.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [0., 0., 2.])))
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_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_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_apply_to_vector_x(self):
# 180 degree turn around X axis
q = quaternion.create_from_x_rotation(np.pi)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0.,-1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0.,-1.]))
# 90 degree rotation around X axis
q = quaternion.create_from_x_rotation(np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0., 0., 1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0.,-1., 0.]))
# -90 degree rotation around X axis
q = quaternion.create_from_x_rotation(-np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0., 0.,-1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 1., 0.]))
def test_apply_to_vector_non_unit(self):
q = quaternion.create_from_x_rotation(np.pi)
# zero length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 0.]), [0., 0., 0.]))
# >1 length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [2., 0., 0.]), [2., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 2., 0.]), [0.,-2., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 2.]), [0., 0.,-2.]))
def test_apply_to_vector_non_unit(self):
q = quaternion.create_from_x_rotation(np.pi)
# zero length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 0.]), [0., 0., 0.]))
# >1 length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [2., 0., 0.]), [2., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 2., 0.]), [0.,-2., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 2.]), [0., 0.,-2.]))
def test_apply_to_vector_x(self):
# 180 degree turn around X axis
q = quaternion.create_from_x_rotation(np.pi)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0.,-1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0.,-1.]))
# 90 degree rotation around X axis
q = quaternion.create_from_x_rotation(np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0., 0., 1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0.,-1., 0.]))
# -90 degree rotation around X axis
q = quaternion.create_from_x_rotation(-np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0., 0.,-1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 1., 0.]))
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_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 rotated_x():
quat = quaternion.create_from_x_rotation( math.pi )
result = matrix33.create_from_quaternion( quat )
expected = matrix33.create_from_x_rotation( math.pi )
self.assertTrue(
numpy.allclose( result, expected ),
"Matrix33 from quaternion incorrect with PI rotation about X"
)
def test_apply_to_vector_x(self):
quat = quaternion.create_from_x_rotation(np.pi / 2.)
self.assertTrue(
np.allclose(quaternion.apply_to_vector(quat, [1., 0., 0.]),
[1., 0., 0.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(quat, [0., 1., 0.]),
[0., 0., -1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(quat, [0., 0., 1.]),
[0., 1., 0.]))
def test_operators_quaternion(self):
q1 = Quaternion()
q2 = Quaternion.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q1 + q2)
# subtract
self.assertRaises(ValueError, lambda: q1 - q2)
# multiply
self.assertTrue(np.array_equal(q1 * q2, quaternion.cross(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# divide
self.assertRaises(ValueError, lambda: q1 / q2)
# or
self.assertTrue(np.array_equal(q1 | q2, quaternion.dot(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# inverse
self.assertTrue(np.array_equal(~q2, quaternion.conjugate(quaternion.create_from_x_rotation(0.5))))
def rotate_x( self, radians ):
"""Pitch the transform about it's X axis.
.. note::
Amount > 0 == pitch down
Amount < 0 == pitch up
"""
if radians == 0.0:
return
quat = quaternion.create_from_x_rotation( radians )
self.rotate_quaternion( quat )
def rotate_x(self, radians):
"""Pitch the transform about it's X axis.
.. note::
Amount > 0 == pitch down
Amount < 0 == pitch up
"""
if radians == 0.0:
return
quat = quaternion.create_from_x_rotation(radians)
self.rotate_quaternion(quat)
def rotateAboutOrigin(self, xyz, t, p):
rp = p - math.pi / 2
rt = -t
q1 = quaternion.create_from_z_rotation(rp)
q2 = quaternion.create_from_x_rotation(rt)
v = vector3.create(xyz[0], xyz[1], xyz[2])
v = quaternion.apply_to_vector(q1, v)
v = quaternion.apply_to_vector(q2, v)
return v
def test_operators_vector4(self):
q = Quaternion.from_x_rotation(0.5)
v = Vector4([1.,0.,0.,1.])
# add
self.assertRaises(ValueError, lambda: q + v)
# subtract
self.assertRaises(ValueError, lambda: q - v)
# multiply
self.assertTrue(np.array_equal(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(0.5), [1.,0.,0.,1.])))
# divide
self.assertRaises(ValueError, lambda: q / v)
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_operators_quaternion(self):
m = Matrix44.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,
matrix44.multiply(
matrix44.create_identity(),
matrix44.create_from_quaternion(
quaternion.create_from_x_rotation(0.7)))))
# divide
self.assertRaises(ValueError, lambda: m / q)
def test_cross(self):
q1 = quaternion.create_from_x_rotation(np.pi / 2.0)
q2 = quaternion.create_from_x_rotation(-np.pi / 2.0)
result = quaternion.cross(q1, q2)
np.testing.assert_almost_equal(result, quaternion.create(), decimal=5)
def test_create_from_inverse_of_quaternion(self):
q = quaternion.create_from_x_rotation(np.pi / 2.0)
result = matrix33.create_from_inverse_of_quaternion(q)
self.assertTrue(
np.allclose(result, matrix33.create_from_x_rotation(-np.pi / 2.0)))
def test_create_from_x_rotation(self):
result = quaternion.create_from_x_rotation(np.pi)
np.testing.assert_almost_equal(result, [1., 0., 0., 0.], decimal=3)
self.assertTrue(result.dtype == np.float)
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_quaternion_rotated_x(self):
quat = quaternion.create_from_x_rotation(np.pi)
result = matrix33.create_from_quaternion(quat)
expected = matrix33.create_from_x_rotation(np.pi)
self.assertTrue(np.allclose(result, expected))
def test_create_from_x_rotation(self):
result = quaternion.create_from_x_rotation(np.pi)
np.testing.assert_almost_equal(result, [1.,0.,0.,0.], decimal=3)
self.assertTrue(result.dtype == np.float)
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_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_identity(), matrix33.create_from_quaternion(quaternion.create_from_x_rotation(0.7)))))
# divide
self.assertRaises(ValueError, lambda: m / q)
def test_cross(self):
q1 = quaternion.create_from_x_rotation(np.pi / 2.0)
q2 = quaternion.create_from_x_rotation(-np.pi / 2.0)
result = quaternion.cross(q1, q2)
np.testing.assert_almost_equal(result, quaternion.create(), decimal=5)
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_create_from_inverse_quaternion(self):
q = Quaternion.from_x_rotation(0.5)
m = Matrix44.from_inverse_of_quaternion(q)
expected = matrix44.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_apply_to_vector_x(self):
quat = quaternion.create_from_x_rotation(np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(quat,[1.,0.,0.]), [1.,0.,0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(quat,[0.,1.,0.]), [0.,0.,-1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(quat,[0.,0.,1.]), [0.,1.,0.]))
def test_create_from_quaternion_rotated_x(self):
quat = quaternion.create_from_x_rotation(np.pi)
result = matrix33.create_from_quaternion(quat)
expected = matrix33.create_from_x_rotation(np.pi)
self.assertTrue(np.allclose(result, expected))
def test_create_from_inverse_of_quaternion(self):
q = quaternion.create_from_x_rotation(np.pi / 2.0)
result = matrix44.create_from_inverse_of_quaternion(q)
self.assertTrue(np.allclose(result, matrix44.create_from_x_rotation(-np.pi / 2.0)))
