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