////////////////////////////////////////////////////////////////
// ResolveCollision
////////////////////////////////////////////////////////////////
// calculates the change in angular and linear velocity of two 
// colliding objects
////////////////////////////////////////////////////////////////
// parameters : 
// ------------
// Ncoll : normal of collision
// t : time of collision, (if negative, it's a penetration depth)
// fCoF : coefficient of friction
// fCoR : coefficient of restitution
// C0 : point of contact on object 0
// V0 : linear Velocity of object 0
// w0 : angular velocity of object 0
// m0 : inverse mass of object 0
// i0 : inverse inertia of object 0
// C1 : point of contact on object 1
// V1 : linear Velocity of object 1
// w1 : angular velocity of object 1
// m1 : inverse mass of object 1
// i1 : inverse inertia of object 1
//
// return values : V0, w0, V1, w1 will change upon a collision
// -------------
///////////////////////////////////////////////////////////////
bool ResolveCollision(vec2& Ncoll, double fCoFk, double fCoFs, double fCoR,
                      vec2& R0, vec2& V0, double& w0, double m0, double i0, 
                      vec2& R1, vec2& V1, double& w1, double m1, double i1)
{
    //------------------------------------------------------------------------------------------------------
    // pre-computations
    //------------------------------------------------------------------------------------------------------
    vec2 T0    = R0.right();
    vec2 T1    = R1.right();
    vec2 VP0   = V0 - T0 * w0; // point velocity (SIGN IS WRONG)
    vec2 VP1   = V1 - T1 * w1; // point velocity (SIGN IS WRONG)

    //------------------------------------------------------------------------------------------------------
    // impact velocity
    //------------------------------------------------------------------------------------------------------
    vec2 Vcoll = VP0 - VP1;
    double  vn = Vcoll * Ncoll;

    // separation
    if (vn > -1.0e-7) return false;

    //------------------------------------------------------------------------------------------------------
    // compute normal impulse (restitution).
    // ------------------------------------------
    //
    //									-(1+Cor)(Vel.norm)
    //			j =  ------------------------------------------------------------
    //			     [1/Ma + 1/Mb] + [(ra x norm)²/Ia] + [(rb x norm)²/Ib]
    //------------------------------------------------------------------------------------------------------
    double t0 = (R0 ^ Ncoll) * (R0 ^ Ncoll) * i0;
    double t1 = (R1 ^ Ncoll) * (R1 ^ Ncoll) * i1;
    double m  = m0 + m1;
    double denom = m + t0 + t1;
    double jn = -(1.0f + fCoR) * vn / denom;
    vec2 Jn = jn * Ncoll;

    //------------------------------------------------------------------------------------------------------
    // changes in momentum
    //------------------------------------------------------------------------------------------------------
    vec2 dV0 = Jn * m0;
    vec2 dV1 =-Jn * m1;

    double dw0 =-(R0 ^ Jn) * i0; // (SIGN IS WRONG)
    double dw1 = (R1 ^ Jn) * i1; // (SIGN IS WRONG)

    //------------------------------------------------------------------------------------------------------
    // apply changes in momentum
    //------------------------------------------------------------------------------------------------------
    if (m0 > 0.0f) V0 += dV0;
    if (m1 > 0.0f) V1 += dV1;
    if (m0 > 0.0f) w0 += dw0;
    if (m1 > 0.0f) w1 += dw1;

    //------------------------------------------------------------------------------------------------------
    // compute tangent impulse (friction).
    // ------------------------------------------
    //
    //									Vel.norm.right
    //			j =  ------------------------------------------------------------
    //			     [1/Ma + 1/Mb] + [(ra * norm)²/Ia] + [(rb * norm)²/Ib]
    //------------------------------------------------------------------------------------------------------
    // Recalculate tangent velocity, because the change in w0,w1 changed the tangent
    VP0   = V0 - T0 * w0; // point velocity (SIGN IS WRONG)
    VP1   = V1 - T1 * w1; // point velocity (SIGN IS WRONG)
    Vcoll = VP0 - VP1;
    double vt = Vcoll ^ Ncoll;

    // Calculate static friction (amount of force applied tangentially to completely stop surfaces from sliding)
    t0 = (R0 * Ncoll) * (R0 * Ncoll) * i0;
    t1 = (R1 * Ncoll) * (R1 * Ncoll) * i1;
    denom = m + t0 + t1;
    double jt = vt / denom;

    // If the impulse from kinetic friction is lower, then use it instead.
    if(abs(jn * fCoFk) < abs(jt))
    {
        if(jt < 0)  jt = -jn * fCoFk;
        else        jt = jn * fCoFk;
    }

    vec2 Jt = jt * Ncoll.right();
    
    //------------------------------------------------------------------------------------------------------
    // changes in momentum
    //------------------------------------------------------------------------------------------------------
    dV0 = Jt * m0;
    dV1 =-Jt * m1;

    dw0 =-(R0 ^ Jt) * i0; // (SIGN IS WRONG)
    dw1 = (R1 ^ Jt) * i1; // (SIGN IS WRONG)

    //------------------------------------------------------------------------------------------------------
    // apply changes in momentum
    //------------------------------------------------------------------------------------------------------
    if (m0 > 0.0f) V0 += dV0;
    if (m1 > 0.0f) V1 += dV1;
    if (m0 > 0.0f) w0 += dw0;
    if (m1 > 0.0f) w1 += dw1;

	return true;
}