public static XMVector BaryCentricV(XMVector q0, XMVector q1, XMVector q2, XMVector f, XMVector g)
{
Debug.Assert(f.Y == f.X && f.Z == f.X && f.W == f.X, "Reviewed");
Debug.Assert(g.Y == g.X && g.Z == g.X && g.W == g.X, "Reviewed");
XMVector epsilon = XMVector.FromSplatConstant(1, 16);
XMVector s = XMVector.Add(f, g);
XMVector result;
if (XMVector4.InBounds(s, epsilon))
{
result = q0;
}
else
{
XMVector q01 = XMQuaternion.SlerpV(q0, q1, s);
XMVector q02 = XMQuaternion.SlerpV(q0, q2, s);
XMVector gs = s.Reciprocal();
gs = XMVector.Multiply(g, gs);
result = XMQuaternion.SlerpV(q01, q02, gs);
}
return(result);
}
public static XMVector SquadV(XMVector q0, XMVector q1, XMVector q2, XMVector q3, XMVector t)
{
Debug.Assert(t.Y == t.X && t.Z == t.X && t.W == t.X, "Reviewed");
XMVector tp = t;
XMVector two = XMVector.FromSplatConstant(2, 0);
XMVector q03 = XMQuaternion.SlerpV(q0, q3, t);
XMVector q12 = XMQuaternion.SlerpV(q1, q2, t);
tp = XMVector.NegativeMultiplySubtract(tp, tp, tp);
tp = XMVector.Multiply(tp, two);
return(XMQuaternion.SlerpV(q03, q12, tp));
}
public static void SquadSetup(out XMVector a, out XMVector b, out XMVector c, XMVector q0, XMVector q1, XMVector q2, XMVector q3)
{
XMVector ls12 = XMQuaternion.LengthSquare(XMVector.Add(q1, q2));
XMVector ld12 = XMQuaternion.LengthSquare(XMVector.Subtract(q1, q2));
XMVector sq2 = q2.Negate();
XMVector control1 = XMVector.Less(ls12, ld12);
sq2 = XMVector.Select(q2, sq2, control1);
XMVector ls01 = XMQuaternion.LengthSquare(XMVector.Add(q0, q1));
XMVector ld01 = XMQuaternion.LengthSquare(XMVector.Subtract(q0, q1));
XMVector sq0 = q0.Negate();
XMVector ls23 = XMQuaternion.LengthSquare(XMVector.Add(sq2, q3));
XMVector ld23 = XMQuaternion.LengthSquare(XMVector.Subtract(sq2, q3));
XMVector sq3 = q3.Negate();
XMVector control0 = XMVector.Less(ls01, ld01);
XMVector control2 = XMVector.Less(ls23, ld23);
sq0 = XMVector.Select(q0, sq0, control0);
sq3 = XMVector.Select(q3, sq3, control2);
XMVector invQ1 = XMQuaternion.Inverse(q1);
XMVector invQ2 = XMQuaternion.Inverse(sq2);
XMVector ln_q0 = XMQuaternion.Ln(XMQuaternion.Multiply(invQ1, sq0));
XMVector ln_q2 = XMQuaternion.Ln(XMQuaternion.Multiply(invQ1, sq2));
XMVector ln_q1 = XMQuaternion.Ln(XMQuaternion.Multiply(invQ2, q1));
XMVector ln_q3 = XMQuaternion.Ln(XMQuaternion.Multiply(invQ2, sq3));
XMVector negativeOneQuarter = XMVector.FromSplatConstant(-1, 2);
XMVector expQ02 = XMVector.Multiply(XMVector.Add(ln_q0, ln_q2), negativeOneQuarter);
XMVector expQ13 = XMVector.Multiply(XMVector.Add(ln_q1, ln_q3), negativeOneQuarter);
expQ02 = XMQuaternion.Exp(expQ02);
expQ13 = XMQuaternion.Exp(expQ13);
a = XMQuaternion.Multiply(q1, expQ02);
b = XMQuaternion.Multiply(sq2, expQ13);
c = sq2;
}
