public void WarmStart(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
{
Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z);
TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB);
PenetrationLimit1.WarmStart(ref projection.Penetration, ref projection.InertiaA, ref projection.InertiaB,
ref projection.Normal,
ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB);
}
public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count,
float dt, float inverseDt, ref Contact1PrestepData prestep, out Contact1Projection projection)
{
bodies.GatherInertia(ref bodyReferences, count, out projection.InertiaA, out projection.InertiaB);
Vector3Wide.Subtract(ref prestep.OffsetA0, ref prestep.OffsetB, out var offsetToManifoldCenterB);
projection.PremultipliedFrictionCoefficient = prestep.FrictionCoefficient;
projection.Normal = prestep.Normal;
Helpers.BuildOrthnormalBasis(ref prestep.Normal, out var x, out var z);
TangentFriction.Prestep(ref x, ref z, ref prestep.OffsetA0, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent);
PenetrationLimit1.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, ref prestep, dt, inverseDt, out projection.Penetration);
}
public void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
{
Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z);
var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * accumulatedImpulses.Penetration0;
TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB);
//Note that we solve the penetration constraints after the friction constraints.
//This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence.
//It's a pretty minor effect either way.
PenetrationLimit1.Solve(ref projection.Penetration, ref projection.InertiaA, ref projection.InertiaB, ref projection.Normal,
ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB);
}