Method warmstart

src/systems/SolveSliderJointSystem.cpp:242–319

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 SolveSliderJointSystem::warmstart() {

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

        const Entity jointEntity = mSliderJointComponents.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 and inverse inertia tensors of the bodies
        const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
        const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];

        const Vector3& n1 = mSliderJointComponents.mN1[i];
        const Vector3& n2 = mSliderJointComponents.mN2[i];

        // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
        decimal impulseLimits = mSliderJointComponents.mImpulseUpperLimit[i] - mSliderJointComponents.mImpulseLowerLimit[i];
        Vector3 linearImpulseLimits = impulseLimits * mSliderJointComponents.mSliderAxisWorld[i];

        // Compute the impulse P=J^T * lambda for the motor constraint of body 1
        Vector3 impulseMotor = mSliderJointComponents.mImpulseMotor[i] * mSliderJointComponents.mSliderAxisWorld[i];

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

        // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
        Vector3 linearImpulseBody1 = -n1 * impulseTranslation.x - n2 * impulseTranslation.y;
        Vector3 angularImpulseBody1 = -mSliderJointComponents.mR1PlusUCrossN1[i] * impulseTranslation.x -
                mSliderJointComponents.mR1PlusUCrossN2[i] * impulseTranslation.y;

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

        // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
        linearImpulseBody1 += linearImpulseLimits;
        angularImpulseBody1 += impulseLimits * mSliderJointComponents.mR1PlusUCrossSliderAxis[i];

        // Compute the impulse P=J^T * lambda for the motor constraint of body 1
        linearImpulseBody1 += impulseMotor;

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