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)