public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertias inertiaA,
out Projection projection)
{
ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians);
//Compute effective mass matrix contributions.
Triangular2x2Wide.SandwichScale(ref jacobians.LinearA, ref inertiaA.InverseMass, out var linearContributionA);
Triangular3x3Wide.MatrixSandwich(ref jacobians.AngularA, ref inertiaA.InverseInertiaTensor, out var angularContributionA);
//No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.)
Triangular2x2Wide.Add(ref linearContributionA, ref angularContributionA, out var inverseEffectiveMass);
Triangular2x2Wide.InvertWithoutOverlap(ref inverseEffectiveMass, out projection.EffectiveMass);
projection.OffsetA = offsetA;
//Note that friction constraints have no bias velocity. They target zero velocity.
}
static void Compare(ref Matrix2x2Wide m, ref Triangular2x2Wide t)
{
var se12 = MeasureError(m.X.Y[0], m.Y.X[0]);
if (se12 > epsilon)
{
throw new Exception("Matrix not symmetric; shouldn't compare against a symmetric triangular matrix.");
}
var e11 = MeasureError(m.X.X[0], t.M11[0]);
var e21 = MeasureError(m.Y.X[0], t.M21[0]);
var e22 = MeasureError(m.Y.Y[0], t.M22[0]);
if (e11 > epsilon ||
e21 > epsilon ||
e22 > epsilon)
{
throw new Exception("Too much error in Matrix2x2Wide vs Triangular2x2Wide.");
}
}
