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)));
}
