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 RotationAxis(XMVector axis, float angle)
{
Debug.Assert(!XMVector3.Equal(axis, XMGlobalConstants.Zero), "Reviewed");
Debug.Assert(!XMVector3.IsInfinite(axis), "Reviewed");
XMVector normal = XMVector3.Normalize(axis);
return(XMQuaternion.RotationNormal(normal, angle));
}
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 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);
}
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 XMVector Inverse(XMVector q)
{
XMVector zero = XMVector.Zero;
XMVector l = XMVector4.LengthSquare(q);
XMVector conjugate = XMQuaternion.Conjugate(q);
XMVector control = XMVector.LessOrEqual(l, XMGlobalConstants.Epsilon);
XMVector result = XMVector.Divide(conjugate, l);
result = XMVector.Select(result, zero, control);
return(result);
}
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;
}
public static XMVector BaryCentric(XMVector q0, XMVector q1, XMVector q2, float f, float g)
{
float s = f + g;
XMVector result;
if (s < 0.00001f && s > -0.00001f)
{
result = q0;
}
else
{
XMVector q01 = XMQuaternion.Slerp(q0, q1, s);
XMVector q02 = XMQuaternion.Slerp(q0, q2, s);
result = XMQuaternion.Slerp(q01, q02, g / s);
}
return(result);
}
public static XMVector RotationRollPitchYaw(float pitch, float yaw, float roll)
{
XMVector angles = new XMVector(pitch, yaw, roll, 0.0f);
return(XMQuaternion.RotationRollPitchYawFromVector(angles));
}
public static XMVector Squad(XMVector q0, XMVector q1, XMVector q2, XMVector q3, float t)
{
XMVector tV = XMVector.Replicate(t);
return(XMQuaternion.SquadV(q0, q1, q2, q3, tV));
}
public static XMVector Slerp(XMVector q0, XMVector q1, float t)
{
XMVector tV = XMVector.Replicate(t);
return(XMQuaternion.SlerpV(q0, q1, tV));
}
