Method warmstart

src/systems/SolveFixedJointSystem.cpp:145–202

Warm start the constraint (apply the previous impulse at the beginning of the step)

// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SolveFixedJointSystem::warmstart() {

    // For each joint
    const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents();
    for (uint32 i=0; i < nbJoints; i++) {

        const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
        const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);

        // Get the bodies entities
        const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
        const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];

        const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
        const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);

        // Get the velocities
        Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
        Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
        Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
        Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];

        // Get the inverse mass of the bodies
        const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
        const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];

        const Vector3& impulseTranslation = mFixedJointComponents.mImpulseTranslation[i];
        const Vector3& impulseRotation = mFixedJointComponents.mImpulseRotation[i];

        const Vector3& r1World = mFixedJointComponents.mR1World[i];
        const Vector3& r2World = mFixedJointComponents.mR2World[i];

        // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
        Vector3 linearImpulseBody1 = -impulseTranslation;
        Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World);

        // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
        angularImpulseBody1 += -impulseRotation;

        const Matrix3x3& i1 = mFixedJointComponents.mI1[i];

        // Apply the impulse to the body 1
        v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
        w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);

        // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
        Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World);

        // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
        angularImpulseBody2 += impulseRotation;

        const Matrix3x3& i2 = mFixedJointComponents.mI2[i];

        // Apply the impulse to the body 2
        v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * impulseTranslation;
        w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
    }
}