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