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); }