Method solve

src/systems/ContactSolverSystem.cpp:479–759

Solve the contacts

// Solve the contacts
void ContactSolverSystem::solve() {

    RP3D_PROFILE("ContactSolverSystem::solve()", mProfiler);

    decimal deltaLambda;
    decimal lambdaTemp;
    uint32 contactPointIndex = 0;

    const decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;

    // For each contact manifold
    for (uint32 c=0; c<mNbContactManifolds; c++) {

        decimal sumPenetrationImpulse = 0.0;

        const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
        const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;

        // Get the constrained velocities
        const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index];
        const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index];
        const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index];
        const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index];

        for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {

            // --------- Penetration --------- //

            // Compute J*v
            //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
            Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x -
                           w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y,
                           v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y -
                           w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z,
                           v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z -
                           w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x);
            decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y +
                                 deltaV.z * mContactPoints[contactPointIndex].normal.z;
            decimal Jv = deltaVDotN;

            // Compute the bias "b" of the constraint
            decimal biasPenetrationDepth = 0.0;
            if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) {
                biasPenetrationDepth = -(beta/mTimeStep) * std::max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP));
            }
            decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias;

            // Compute the Lagrange multiplier lambda
            if (mIsSplitImpulseActive) {
                deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) *
                        mContactPoints[contactPointIndex].inversePenetrationMass;
            }
            else {
                deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass;
            }
            lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse;
            mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse +
                                                       deltaLambda, decimal(0.0));