ResolveContact

/// <summary>
/// Resolve single contact
///
/// Performs contact resolution by applying forces to bodies and nudging them back a bit (in case they penetrate)
/// </summary>
/// <param name="contact">Contact (collision) to resolve</param>
void Scene::ResolveContact(Contact& contact)
{
    Body* bodyA = contact.mBodyA;
    Body* bodyB = contact.mBodyB;

    const float3 pointOnA = bodyA->ModelToWorldSpace(contact.mPointOnA_ModelSpace);
    const float3 pointOnB = bodyB->ModelToWorldSpace(contact.mPointOnB_ModelSpace);

    const float elasticity = bodyA->mResitution * bodyB->mResitution;

    const float invMassA = bodyA->mInvMass;
    const float invMassB = bodyB->mInvMass;

    const mat3 invWorldInertiaA = bodyA->GetInverseInertiaTensorWorldSpace();
    const mat3 invWorldInertiaB = bodyB->GetInverseInertiaTensorWorldSpace();

    const float3 normal = contact.mNormal;

    const float3 radiusA = pointOnA - bodyA->GetCenterOfMassWorldSpace();
    const float3 radiusB = pointOnB - bodyB->GetCenterOfMassWorldSpace();

    const float3 angularImpulseA = cross(invWorldInertiaA * cross(radiusA, normal), radiusA);
    const float3 angularImpulseB = cross(invWorldInertiaB * cross(radiusB, normal), radiusB);
    const float angularFactor = dot(angularImpulseA + angularImpulseB, normal);

    // Obtain world space velocity of the motion and rotation
    const float3 velocityA = bodyA->mLinearVelocity + cross(bodyA->mAngularVelocity, radiusA);
    const float3 velocityB = bodyB->mLinearVelocity + cross(bodyB->mAngularVelocity, radiusB);

    // Calculate collision impulse
    const float3 velocityAB = velocityA - velocityB;
    const float impulse = (1.0f + elasticity) * dot(velocityAB, normal) / (bodyA->mInvMass + bodyB->mInvMass + angularFactor);
    const float3 impulseVec = normal * impulse;

    bodyA->ApplyImpulse(pointOnA, impulseVec * -1.0f);
    bodyB->ApplyImpulse(pointOnB, impulseVec * 1.0f);

    // Calculate the impulse caused by friction
    const float friction = bodyA->mFriction * bodyB->mFriction;

    // Find normal direction of the velocity (respecting normal of the collision)
    const float3 velocityNormal = normal * dot(normal, velocityAB);
    // Find tangent direction of the velocity (respecting normal of the collision)
    const float3 velocityTangent = velocityAB - velocityNormal;

    // Get tangential velocities direction to the other body
    float velocityTangentMagnitude = length(velocityTangent);
    // Static friction under certain threshold???
    if (velocityTangentMagnitude < 0.01f)
    {
        /*bodyA->mAngularVelocity *= 0.9f;
        bodyB->mAngularVelocity *= 0.9f;

        bodyA->mLinearVelocity *= 0.9f;
        bodyB->mLinearVelocity *= 0.9f;*/
    }
    else
    {
        float3 tangentVec = velocityTangent / velocityTangentMagnitude;

        const float3 inertiaA = cross(invWorldInertiaA * cross(radiusA, tangentVec), radiusA);
        const float3 inertiaB = cross(invWorldInertiaB * cross(radiusB, tangentVec), radiusB);
        const float inverseInertia = dot(inertiaA + inertiaB, tangentVec);

        // Calculate tangential impulse for friction
        const float reducedMass = 1.0f / (bodyA->mInvMass + bodyB->mInvMass + inverseInertia);
        const float3 impulseFriction = velocityTangent * reducedMass * friction;

        // Apply friction
        bodyA->ApplyImpulse(pointOnA, impulseFriction * -1.0f);
        bodyB->ApplyImpulse(pointOnB, impulseFriction * 1.0f);
    }

    // Move colliding objects to just outside of each otehr
#ifdef CCD
    if (contact.mTimeOfImpact == 0.0f)
    {
        // Resolve by moving collision objects to just outside of each other
        const float tA = bodyA->mInvMass / (bodyA->mInvMass + bodyB->mInvMass);
        const float tB = bodyB->mInvMass / (bodyA->mInvMass + bodyB->mInvMass);

        const float3 ds = pointOnB - pointOnA;

        bodyA->mPosition += ds * tA;
        bodyB->mPosition -= ds * tB;
    }
#else
    const float tA = bodyA->mInvMass / (bodyA->mInvMass + bodyB->mInvMass);
    const float tB = bodyB->mInvMass / (bodyA->mInvMass + bodyB->mInvMass);

    const float3 ds = pointOnB - pointOnA;

    bodyA->mPosition += ds * tA;
    bodyB->mPosition -= ds * tB;
#endif
}