public static XMVector InverseRotate(XMVector v, XMVector rotationQuaternion)
{
//// Transform a vector using the inverse of a rotation expressed as a unit quaternion
XMVector a = XMVector.Select(XMGlobalConstants.Select1110, v, XMGlobalConstants.Select1110);
XMVector result = XMQuaternion.Multiply(rotationQuaternion, a);
XMVector q = XMQuaternion.Conjugate(rotationQuaternion);
return(XMQuaternion.Multiply(result, q));
}
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;
}