public void SquadDoublePrecision()
        {
            QuaternionD q0 = QuaternionD.CreateRotation(new Vector3D(1, 1, 1), 0.3);
            QuaternionD q1 = QuaternionD.CreateRotation(new Vector3D(1, 0, 1), 0.4);
            QuaternionD q2 = QuaternionD.CreateRotation(new Vector3D(1, 0, -1), -0.6);
            QuaternionD q3 = QuaternionD.CreateRotation(new Vector3D(0, 1, 1), 0.2);

            QuaternionD q, a, b, p;
            QuaternionD expected;

            InterpolationHelper.SquadSetup(q0, q1, q2, q3, out q, out a, out b, out p);

            // t = 0
            QuaternionD result = InterpolationHelper.Squad(q, a, b, p, 0.0);

            Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, result));

            // t = 1.0f
            result = InterpolationHelper.Squad(q, a, b, p, 1.0);
            Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, result));

            // Check series (just for debugging)
            QuaternionD r1, r2, r3, r4, r5, r6, r7, r8, r9;

            r1 = InterpolationHelper.Squad(q, a, b, p, 0.1);
            r2 = InterpolationHelper.Squad(q, a, b, p, 0.2);
            r3 = InterpolationHelper.Squad(q, a, b, p, 0.3);
            r4 = InterpolationHelper.Squad(q, a, b, p, 0.4);
            r5 = InterpolationHelper.Squad(q, a, b, p, 0.5);
            r6 = InterpolationHelper.Squad(q, a, b, p, 0.6);
            r7 = InterpolationHelper.Squad(q, a, b, p, 0.7);
            r8 = InterpolationHelper.Squad(q, a, b, p, 0.8);
            r9 = InterpolationHelper.Squad(q, a, b, p, 0.9);

            // q0 = q1, q2 = q3
            InterpolationHelper.SquadSetup(q1, q1, q2, q2, out q, out a, out b, out p);
            result   = InterpolationHelper.Squad(q, a, b, p, 0.5);
            expected = InterpolationHelper.Slerp(q1, q2, 0.5);
            Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, result));
        }
        public void SquadSinglePrecision()
        {
            Quaternion q0 = Quaternion.CreateFromRotationMatrix(new Vector3(1, 1, 1), 0.3f);
            Quaternion q1 = Quaternion.CreateFromRotationMatrix(new Vector3(1, 0, 1), 0.4f);
            Quaternion q2 = Quaternion.CreateFromRotationMatrix(new Vector3(1, 0, -1), -0.6f);
            Quaternion q3 = Quaternion.CreateFromRotationMatrix(new Vector3(0, 1, 1), 0.2f);

            Quaternion q, a, b, p;
            Quaternion expected;

            InterpolationHelper.SquadSetup(q0, q1, q2, q3, out q, out a, out b, out p);

            // t = 0
            Quaternion result = InterpolationHelper.Squad(q, a, b, p, 0.0f);

            Assert.IsTrue(Quaternion.AreNumericallyEqual(q1, result));

            // t = 1.0f
            result = InterpolationHelper.Squad(q, a, b, p, 1.0f);
            Assert.IsTrue(Quaternion.AreNumericallyEqual(q2, result));

            // Check series (just for debugging)
            Quaternion r1, r2, r3, r4, r5, r6, r7, r8, r9;

            r1 = InterpolationHelper.Squad(q, a, b, p, 0.1f);
            r2 = InterpolationHelper.Squad(q, a, b, p, 0.2f);
            r3 = InterpolationHelper.Squad(q, a, b, p, 0.3f);
            r4 = InterpolationHelper.Squad(q, a, b, p, 0.4f);
            r5 = InterpolationHelper.Squad(q, a, b, p, 0.5f);
            r6 = InterpolationHelper.Squad(q, a, b, p, 0.6f);
            r7 = InterpolationHelper.Squad(q, a, b, p, 0.7f);
            r8 = InterpolationHelper.Squad(q, a, b, p, 0.8f);
            r9 = InterpolationHelper.Squad(q, a, b, p, 0.9f);

            // q0 = q1, q2 = q3
            InterpolationHelper.SquadSetup(q1, q1, q2, q2, out q, out a, out b, out p);
            result   = InterpolationHelper.Squad(q, a, b, p, 0.5f);
            expected = InterpolationHelper.Slerp(q1, q2, 0.5f);
            Assert.IsTrue(Quaternion.AreNumericallyEqual(expected, result));
        }