/// <summary>
/// Given 2 Physics Entities that are known to have overlapping colliders, seperates them by shifting their position
/// based on the collision normal and penetration depth
/// </summary>
/// <param name="PenetrationDepth"> The depth of penetration of the PhysicsEntitites in question </param>
/// <param name="A"> The first physics Entity </param>
/// <param name="B"> The second physics Entity </param>
/// <param name="normal"> The collision normal </param>
public static void CorrectPosition(float PenetrationDepth, PhysicsEntity A, PhysicsEntity B, Vector normal)
{
float percent = 0.6f;
float tolerance = 0.2f;
Vector Correction = normal * (Math.Max(PenetrationDepth - tolerance, 0.0f) / ((A.GetInvMass() + B.GetInvMass())) * percent);
Vector bef = A.position;
if (Mathf.Abs(Correction.x) > 100 || Mathf.Abs(Correction.y) > 100)
{
return;
}
A.position.Add(Correction * A.GetInvMass());
B.position.Add(Correction * -B.GetInvMass());
bef = bef - A.position;
}
/// <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);
}
}
/// <summary>
/// Checks if the given PhysicsEntities are both infintely massed
/// </summary>
/// <returns></returns>
private static bool IsEntitiesInfinitelyMassed(PhysicsEntity A, PhysicsEntity B)
{
return(Utils.IsNearEqual(A.GetInvMass(), 0, 0.001f) && Utils.IsNearEqual(B.GetInvMass(), 0, 0.001f));
}