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