public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians,
ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
{
Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear);
Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular);
Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Linear, jacobians.LinearA, out var csvbLinear);
Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Angular, jacobians.AngularB, out var csvbAngular);
//Note that the velocity in constraint space is (csvaLinear - csvbLinear + csvaAngular + csvbAngular).
//The subtraction there is due to sharing the linear jacobian between both bodies3D.
//In the following, we need to compute the constraint space *violating* velocity- which is the negation of the above velocity in constraint space.
//So, (csvbLinear - csvaLinear - (csvaAngular + csvbAngular)).
Vector2Wide.Subtract(csvbLinear, csvaLinear, out var csvLinear);
Vector2Wide.Add(csvaAngular, csvbAngular, out var csvAngular);
Vector2Wide.Subtract(csvLinear, csvAngular, out var csv);
Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var csi);
var previousAccumulated = accumulatedImpulse;
Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse);
//The maximum force of friction depends upon the normal impulse. The maximum is supplied per iteration.
Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude);
//Note division by zero guard.
var scale = Vector.Min(Vector <float> .One, maximumImpulse / Vector.Max(new Vector <float>(1e-16f), accumulatedMagnitude));
Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse);
Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI);
}
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.
Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA);
Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, 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.)
Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass);
Symmetric2x2Wide.InvertWithoutOverlap(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 Symmetric2x2Wide 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.XX[0]);
var e21 = MeasureError(m.Y.X[0], t.YX[0]);
var e22 = MeasureError(m.Y.Y[0], t.YY[0]);
if (e11 > epsilon ||
e21 > epsilon ||
e22 > epsilon)
{
throw new Exception("Too much error in Matrix2x2Wide vs Triangular2x2Wide.");
}
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Projection data, ref Jacobians jacobians,
ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
{
Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear);
Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular);
Vector2Wide.Add(csvaLinear, csvaAngular, out var csv);
//Required corrective velocity is the negation of the current constraint space velocity.
Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var negativeCSI);
var previousAccumulated = accumulatedImpulse;
Vector2Wide.Subtract(accumulatedImpulse, negativeCSI, out accumulatedImpulse);
//The maximum force of friction depends upon the normal impulse. The maximum is supplied per iteration.
Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude);
//Note division by zero guard.
var scale = Vector.Min(Vector <float> .One, maximumImpulse / Vector.Max(new Vector <float>(1e-16f), accumulatedMagnitude));
Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse);
Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI);
}
