public void GetAngleTest() { QuaternionD qIdentity = QuaternionD.Identity; QuaternionD q03 = QuaternionD.CreateRotation(Vector3D.UnitX, 0.3); QuaternionD q03Plus11 = QuaternionD.CreateRotation(new Vector3D(1, 0.2, -3), 1.1) * q03; QuaternionD q0 = QuaternionD.CreateRotation(Vector3D.UnitX, 0.0); QuaternionD qPi = QuaternionD.CreateRotation(Vector3D.UnitX, ConstantsD.Pi); QuaternionD q2Pi = QuaternionD.CreateRotation(Vector3D.UnitX, ConstantsD.TwoPi); Assert.IsTrue(Numeric.AreEqual(0.0, QuaternionD.GetAngle(qIdentity, qIdentity))); Assert.IsTrue(Numeric.AreEqual(0.3, QuaternionD.GetAngle(qIdentity, q03))); Assert.IsTrue(Numeric.AreEqual(0.3, QuaternionD.GetAngle(qIdentity, -q03))); // Remember: q and -q represent the same orientation. Assert.IsTrue(Numeric.AreEqual(1.1, QuaternionD.GetAngle(q03, q03Plus11))); Assert.IsTrue(Numeric.AreEqual(1.1, QuaternionD.GetAngle(-q03, q03Plus11))); Assert.IsTrue(Numeric.AreEqual(1.1, QuaternionD.GetAngle(q03, -q03Plus11))); Assert.IsTrue(Numeric.AreEqual(1.1, QuaternionD.GetAngle(-q03, -q03Plus11))); Assert.IsTrue(Numeric.AreEqual(0.0, QuaternionD.GetAngle(qIdentity, q0))); Assert.IsTrue(Numeric.AreEqual(0.0, QuaternionD.GetAngle(qIdentity, q2Pi))); Assert.IsTrue(Numeric.AreEqual(0.0, QuaternionD.GetAngle(q0, q2Pi))); Assert.IsTrue(Numeric.AreEqual(0.3, QuaternionD.GetAngle(q03, q0))); Assert.IsTrue(Numeric.AreEqual(ConstantsD.Pi, QuaternionD.GetAngle(q0, qPi))); Assert.IsTrue(Numeric.AreEqual(ConstantsD.Pi, QuaternionD.GetAngle(q2Pi, qPi))); }