private static void ApplyPositionalCorrection(Rigidbody bodyA, Rigidbody bodyB, KVector2 wNormal, double wPenetration, KVector2 wPosition)
{
KVector2 rA = wPosition - bodyA.State.Transform.TransformPointLW(bodyA.MassData.CenterOfMass);
KVector2 rB = wPosition - bodyB.State.Transform.TransformPointLW(bodyB.MassData.CenterOfMass);
double rAXwNormal = (bodyA.MassData.InvInertia * (rA % wNormal) * (rA % wNormal));
double rBXwNormal = (bodyB.MassData.InvInertia * (rA % wNormal) * (rB % wNormal));
double MassEff = bodyA.MassData.InvMass + bodyB.MassData.InvMass;
double impulse = MassEff > 0f ? wPenetration / MassEff : 0f;
KVector2 linMomentum = impulse * wNormal;
bodyA.AddImpulseAtRelPosToCenter(-linMomentum, rA);
bodyB.AddImpulseAtRelPosToCenter(linMomentum, rB);
}
private static void ApplyVelocityChange(Rigidbody bodyA, Rigidbody bodyB, KVector2 wNormal, KVector2 wPosition, KVector2 relVitAB, int count)
{
KVector2 rA = wPosition - bodyA.State.Transform.TransformPointLW(bodyA.MassData.CenterOfMass);
KVector2 rB = wPosition - bodyB.State.Transform.TransformPointLW(bodyB.MassData.CenterOfMass);
double rAXwNormal = (bodyA.MassData.InvInertia * (rA % wNormal) * (rA % wNormal));
double rBXwNormal = (bodyB.MassData.InvInertia * (rB % wNormal) * (rB % wNormal));
double MassEff = bodyA.MassData.InvMass + bodyB.MassData.InvMass + rAXwNormal + rBXwNormal;
double j = -(1 + Math.Min(bodyA.Materiel.restitution, bodyB.Materiel.restitution)) * (relVitAB) * wNormal;
double impulse = MassEff > Config.EpsilonsFloat ? j / MassEff : 0f;
KVector2 momentum = impulse * wNormal;
bodyA.AddImpulseAtRelPosToCenter(-momentum, rA);
bodyB.AddImpulseAtRelPosToCenter(momentum, rB);
}