Example #1
0
        public static XMVector SlerpV(XMVector q0, XMVector q1, XMVector t)
        {
            Debug.Assert(t.Y == t.X && t.Z == t.X && t.W == t.X, "Reviewed");

            //// Result = Q0 * sin((1.0 - t) * Omega) / sin(Omega) + Q1 * sin(t * Omega) / sin(Omega)

            XMVector oneMinusEpsilon = XMVector.FromFloat(1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f);

            XMVector cosOmega = XMQuaternion.Dot(q0, q1);
            XMVector zero     = XMVector.Zero;
            XMVector control  = XMVector.Less(cosOmega, zero);
            XMVector sign     = XMVector.Select(XMGlobalConstants.One, XMGlobalConstants.NegativeOne, control);

            cosOmega = XMVector.Multiply(cosOmega, sign);
            control  = XMVector.Less(cosOmega, oneMinusEpsilon);

            XMVector sinOmega = XMVector
                                .NegativeMultiplySubtract(cosOmega, cosOmega, XMGlobalConstants.One)
                                .Sqrt();

            XMVector omega = XMVector.ATan2(sinOmega, cosOmega);

            XMVector signMask = XMVector.SignMask;
            XMVector v01      = XMVector.ShiftLeft(t, zero, 2);

            signMask = XMVector.ShiftLeft(signMask, zero, 3);
            v01      = XMVector.XorInt(v01, signMask);
            v01      = XMVector.Add(XMGlobalConstants.IdentityR0, v01);

            XMVector invSinOmega = sinOmega.Reciprocal();

            XMVector s0 = XMVector
                          .Multiply(v01, omega)
                          .Sin();

            s0 = XMVector.Multiply(s0, invSinOmega);
            s0 = XMVector.Select(v01, s0, control);

            XMVector s1 = XMVector.SplatY(s0);

            s0 = XMVector.SplatX(s0);
            s1 = XMVector.Multiply(s1, sign);

            XMVector result = XMVector.Multiply(q0, s0);

            result = XMVector.MultiplyAdd(q1, s1, result);

            return(result);
        }