Method initBeforeSolve

src/systems/SolveSliderJointSystem.cpp:48–239

Initialize before solving the constraint

// Initialize before solving the constraint
void SolveSliderJointSystem::initBeforeSolve() {

    const decimal biasFactor = (BETA / mTimeStep);

    // For each joint
    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);

        assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity) || !mRigidBodyComponents.getIsEntityDisabled(body2Entity));

        // Get the inertia tensor of bodies
        mSliderJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1];
        mSliderJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2];

        const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
        const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();

        // Vector from body center to the anchor point
        mSliderJointComponents.mR1[i] = orientationBody1 * (mSliderJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]);
        mSliderJointComponents.mR2[i] = orientationBody2 * (mSliderJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]);

        // Compute the two orthogonal vectors to the slider axis in world-space
        mSliderJointComponents.mSliderAxisWorld[i] = orientationBody1 * mSliderJointComponents.mSliderAxisBody1[i];
        mSliderJointComponents.mSliderAxisWorld[i].normalize();

        mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector();
        mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]);

        const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
        const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2];

        const Vector3& r1 = mSliderJointComponents.mR1[i];
        const Vector3& r2 = mSliderJointComponents.mR2[i];

        // Compute the vector u (difference between anchor points)
        const Vector3 u = x2 + r2 - x1 - r1;

        // Compute the cross products used in the Jacobians
        const Vector3 r1PlusU = mSliderJointComponents.mR1[i] + u;
        mSliderJointComponents.mR1PlusUCrossN1[i] = r1PlusU.cross(mSliderJointComponents.mN1[i]);
        mSliderJointComponents.mR1PlusUCrossN2[i] = r1PlusU.cross(mSliderJointComponents.mN2[i]);
        mSliderJointComponents.mR1PlusUCrossSliderAxis[i] = r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i]);

        // Check if the limit constraints are violated or not
        decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]);
        decimal lowerLimitError = uDotSliderAxis - mSliderJointComponents.mLowerLimit[i];
        decimal upperLimitError = mSliderJointComponents.mUpperLimit[i] - uDotSliderAxis;
        bool oldIsLowerLimitViolated = mSliderJointComponents.mIsLowerLimitViolated[i];