Sign in to follow this  
pTymN

2D collision response [free code]

Recommended Posts

pTymN    464
Hello, Here's a modified version of oliii's polycolly ResolveCollision. It has better friction.
////////////////////////////////////////////////////////////////
// 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;
}
I'm still missing a way to specify the static friction cone, but even currently, the improved friction code much better. Thank you oliii for polycolly!

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this