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