/// <summary>
/// Given a manifold containing all of the required information to resolve a known collision between 2 Colliders,
/// resolves the collision
/// </summary>
/// <param name="m"> A Manifold containing the required collision information </param>
public static void ResolveCollisions(Manifold m)
{
PhysicsEntity A = m.A;
PhysicsEntity B = m.B;
if (m.IsEmpty || IsEntitiesInfinitelyMassed(A, B))
{
return;
}
float e = Mathf.Min(m.A.Restitution, m.B.Restitution);
float AAngVelo = Mathf.Deg2Rad(angularFlipperException(A));
float BAngVelo = Mathf.Deg2Rad(angularFlipperException(B));
Vector BVel = B.velocity;
Vector AVel = A.velocity;
for (int i = 0; i < m.ContactPoint.Count; i++)
{
Vector rAP = m.ContactPoint[i] - A.GetCenterOfMass();
Vector rBP = m.ContactPoint[i] - B.GetCenterOfMass();
//get relative velocity
Vector AB = BVel + (Vector.Cross(BAngVelo, rBP)) - AVel - (Vector.Cross(AAngVelo, rAP));
Vector normal = m.Normal.Normalized();
CorrectPosition(m.PenetrationDepth, A, B, -1 * normal);
//get relative velocity along the normal
float NormalVelocity = Vector.Dot(AB, normal);
//if they are seperating, do not resolve
if (NormalVelocity > 0)
{
return;
}
float jNumerator = -1 * (1f + e) * NormalVelocity;
float rAPCross = Vector.Cross(rAP, normal);
float rBPCross = Vector.Cross(rBP, normal);
float jDenominator = A.GetInvMass() + B.GetInvMass() + (Mathf.Pow(rAPCross, 2) * A.GetInvInertia()) + (Mathf.Pow(rBPCross, 2) * B.GetInvInertia());
float j = jNumerator / jDenominator;
j /= m.ContactPoint.Count;
A.ApplyImpulse(-j * normal, rAP);
B.ApplyImpulse(j * normal, rBP);
//FRICTION
Vector tangent = AB - (normal * Vector.Dot(AB, normal));
tangent.Normalize();
//find jt
float jTangent = -Vector.Dot(AB, tangent);
jTangent = jTangent / jDenominator;
//spread out friction based on contact point
jTangent = jTangent / m.ContactPoint.Count;
float mu = SetFriction(A.GetStaticFriction(), B.GetStaticFriction());
Vector frictionImpulse;
//Ffriciton < normal * mu
if (Mathf.Abs(jTangent) < j * mu)
{
//force applied on object did not pass activation force
frictionImpulse = jTangent * tangent;
}
else
{
//force applied on object passes activation force
float dynamicFriction = SetFriction(A.GetDynamicFriction(), B.GetDynamicFriction());
frictionImpulse = -j * tangent * dynamicFriction;
}
A.ApplyImpulse(-1 * frictionImpulse, rAP);
B.ApplyImpulse(frictionImpulse, rBP);
}
}
