Method warmStart

src/systems/ContactSolverSystem.cpp:326–476

Warm start the solver. For each constraint, we apply the previous impulse (from the previous step) at the beginning. With this technique, we will converge faster towards the solution of the linear system

/// at the beginning. With this technique, we will converge faster towards
/// the solution of the linear system
void ContactSolverSystem::warmStart() {

    RP3D_PROFILE("ContactSolver::warmStart()", mProfiler);

    uint32 contactPointIndex = 0;

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

        bool atLeastOneRestingContactPoint = false;

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

            // If it is not a new contact (this contact was already existing at last time step)
            if (mContactPoints[contactPointIndex].isRestingContact) {

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

                atLeastOneRestingContactPoint = true;

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

                // Update the velocities of the body 1 by applying the impulse P
                Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
                                           mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
                                           mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x * mContactConstraints[c].linearLockAxisFactorBody1.x;
                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y * mContactConstraints[c].linearLockAxisFactorBody1.y;
                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z * mContactConstraints[c].linearLockAxisFactorBody1.z;

                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactConstraints[c].angularLockAxisFactorBody1.x * mContactPoints[contactPointIndex].penetrationImpulse;
                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactConstraints[c].angularLockAxisFactorBody1.y * mContactPoints[contactPointIndex].penetrationImpulse;
                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactConstraints[c].angularLockAxisFactorBody1.z * mContactPoints[contactPointIndex].penetrationImpulse;

                // Update the velocities of the body 2 by applying the impulse P
                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x * mContactConstraints[c].linearLockAxisFactorBody2.x;
                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y * mContactConstraints[c].linearLockAxisFactorBody2.y;
                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z * mContactConstraints[c].linearLockAxisFactorBody2.z;

                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactConstraints[c].angularLockAxisFactorBody2.x * mContactPoints[contactPointIndex].penetrationImpulse;
                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactConstraints[c].angularLockAxisFactorBody2.y * mContactPoints[contactPointIndex].penetrationImpulse;
                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactConstraints[c].angularLockAxisFactorBody2.z * mContactPoints[contactPointIndex].penetrationImpulse;
            }
            else {  // If it is a new contact point

                // Initialize the accumulated impulses to zero
                mContactPoints[contactPointIndex].penetrationImpulse = 0.0;
            }

            contactPointIndex++;
        }

        // If we solve the friction constraints at the center of the contact manifold and there is
        // at least one resting contact point in the contact manifold
        if (atLeastOneRestingContactPoint) {

            // Project the old friction impulses (with old friction vectors) into the new friction