def test_normalisation(self): # normalise to unit quaternion
r = randomElements()
q1 = Quaternion(*r)
v = q1.unit()
n = q1.normalised()
if q1 == Quaternion(0): # small chance with random generation
return # a 0 quaternion does not normalise
# Test normalised objects are unit quaternions
np.testing.assert_almost_equal(v.q, q1.elements() / q1.norm(), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(n.q, q1.elements() / q1.norm(), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(v.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(n.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
# Test axis and angle remain the same
np.testing.assert_almost_equal(q1.axis(), v.axis(), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q1.axis(), n.axis(), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q1.angle(), v.angle(), ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q1.angle(), n.angle(), ALMOST_EQUAL_TOLERANCE)
# Test special case where q is zero
q2 = Quaternion(0)
self.assertEqual(q2, q2.normalised())
def validate_axis_angle(self, axis, angle):
def wrap_angle(theta):
""" Wrap any angle to lie between -pi and pi
Odd multiples of pi are wrapped to +pi (as opposed to -pi)
"""
result = ((theta + pi) % (2*pi)) - pi
if result == -pi: result = pi
return result
theta = wrap_angle(angle)
v = axis
q = Quaternion(angle=theta, axis=v)
v_ = q.axis()
theta_ = q.angle()
if theta == 0.0: # axis is irrelevant (check defaults to x=y=z)
np.testing.assert_almost_equal(theta_, 0.0, decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(v_, np.zeros(3), decimal=ALMOST_EQUAL_TOLERANCE)
return
elif abs(theta) == pi: # rotation in either direction is equivalent
self.assertTrue(
np.isclose(theta, pi) or np.isclose(theta, -pi)
and
np.isclose(v, v_).all() or np.isclose(v, -v_).all()
)
else:
self.assertTrue(
np.isclose(theta, theta_) and np.isclose(v, v_).all()
or
np.isclose(theta, -theta_) and np.isclose(v, -v_).all()
)
# Ensure the returned axis is a unit vector
np.testing.assert_almost_equal(np.linalg.norm(v_), 1.0, decimal=ALMOST_EQUAL_TOLERANCE)
