public void AddCollisionPair(CollisionPairData pair) { RigidBody A = pair.BodyA; RigidBody B = pair.BodyB; //Only add the new pair if it is not already present in the list (avoid resolving the same collision twice) foreach (var p in mCollisionPairs) { if ((Object.ReferenceEquals(A, p.BodyA) && Object.ReferenceEquals(B, p.BodyB)) || (Object.ReferenceEquals(A, p.BodyB) && Object.ReferenceEquals(B, p.BodyA))) { return; } } mCollisionPairs.Add(pair); }
public void Resolve(CollisionPairData data) { //if neither object reacts to force then neither should be affected by the collision if (!data.BodyA.reactToForce && !data.BodyB.reactToForce) { return; } //COLLISION RESOLUTION //Credit goes to http://www.randygaul.net/2013/03/27/game-physics-engine-part-1-impulse-resolution/ for many of the algorithms used in this method //Calculate and resolve the collision described by the CollisionPairData Vector2D combinedVelocity = data.BodyA.LinearVelocity - data.BodyB.LinearVelocity; //normalise the contact normal data.ContactNormal = data.ContactNormal.Normalised(); float combinedVelInNormal = combinedVelocity.Dot(data.ContactNormal); //Do nothing if the objects are moving away from each other if (combinedVelInNormal > 0) { return; } float elasticity = Math.Min(data.BodyA.elasticity, data.BodyB.elasticity); float invMassA = 1 / data.BodyA.Mass; float invMassB = 1 / data.BodyB.Mass; //if either object doesn't react to collisions, treat its mass as infinite if (!data.BodyA.reactToForce) { invMassA = 0; } if (!data.BodyB.reactToForce) { invMassB = 0; } //Calculate the impulse float impulseN = -1 * (1 + elasticity) * combinedVelInNormal; impulseN /= invMassA + invMassB; //Calculate the new velocities for both bodies Vector2D newAVelocity = data.BodyA.LinearVelocity + data.ContactNormal * impulseN * invMassA; Vector2D newBVelocity = data.BodyB.LinearVelocity - data.ContactNormal * impulseN * invMassB; data.BodyA.collisionNormal = data.ContactNormal; data.BodyA.otherBody = data.BodyB; data.BodyB.collisionNormal = data.ContactNormal * -1; data.BodyB.otherBody = data.BodyA; //Calculate the amount each body needs to move away from the other to be properly separated float penetrationAllowance = 0.01f; float percentageToMove = 1f; float distanceToMove = Math.Max(data.BodyA.Shape.GetCollisionPenDepth(data.BodyB.Shape, data.BodyA.Position, data.BodyB.Position) - penetrationAllowance, 0.0f) * percentageToMove; Vector2D displacement = data.ContactNormal * distanceToMove; //React differently if either body is static if (data.BodyA.reactToForce && data.BodyB.reactToForce) { //Move each body away from the other by a percentage of the calculated penetration depth //This percentage is determined by the ratio between the bodies masses data.BodyA.LinearVelocity = newAVelocity; data.BodyA.SetPosition(data.BodyA.Position + (displacement * data.BodyB.Mass / (data.BodyA.Mass + data.BodyB.Mass))); data.BodyB.LinearVelocity = newBVelocity; data.BodyB.SetPosition(data.BodyB.Position - (displacement * data.BodyA.Mass / (data.BodyA.Mass + data.BodyB.Mass))); } else if (!data.BodyA.reactToForce) { //reflect the velocity in the normal (reflection = direction - 2(direction.normal) * normal) data.BodyB.LinearVelocity = newBVelocity; data.BodyB.SetPosition(data.BodyB.Position - displacement); } else if (!data.BodyB.reactToForce) { //reflect the velocity in the normal (reflection = direction - 2(direction.normal) * normal) data.BodyA.LinearVelocity = newAVelocity; data.BodyA.SetPosition(data.BodyA.Position + displacement); } //FRICTION RESOLUTION //apply friction to each of the two colliding bodies //using coulomb friction as described in https://gamedevelopment.tutsplus.com/tutorials/how-to-create-a-custom-2d-physics-engine-friction-scene-and-jump-table--gamedev-7756 float staticFrictionA = data.BodyA.staticFriction, staticFrictionB = data.BodyB.staticFriction; float dynamicFrictionA = data.BodyA.dynamicFriction, dynamicFrictionB = data.BodyB.dynamicFriction; Vector2D tangent = combinedVelocity - data.ContactNormal * combinedVelInNormal; tangent = tangent.Normalised(); //Calculate the impulse float impulseT = -1 * combinedVelocity.Dot(tangent); impulseT /= invMassA + invMassB; float mu = (float)Math.Sqrt(staticFrictionA * staticFrictionA + staticFrictionB * staticFrictionB); Vector2D frictionImpulse; if (Math.Abs(impulseT) < impulseN * mu) { frictionImpulse = tangent * impulseT; } else { float dynamicFriction = (float)Math.Sqrt(dynamicFrictionA * dynamicFrictionA + dynamicFrictionB * dynamicFrictionB); frictionImpulse = tangent * -1 * impulseN * dynamicFriction; } data.BodyA.LinearVelocity = data.BodyA.LinearVelocity + frictionImpulse * invMassA; data.BodyB.LinearVelocity = data.BodyB.LinearVelocity - frictionImpulse * invMassB; }