Start working of the SliderJoint limits

src/constraint/SliderJoint.cpp

@@ -30,11 +30,23 @@ using namespace reactphysics3d;
 
 // Constructor
 SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
-            : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0) {
+            : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
+              mImpulseLowerLimit(0), mIsLimitsActive(jointInfo.isLimitsActive),
+              mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit) {
+
+    assert(mUpperLimit >= 0.0);
+    assert(mLowerLimit <= 0.0);
 
     // Compute the local-space anchor point for each body
-    mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+    const Transform& transform1 = mBody1->getTransform();
-    mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+    const Transform& transform2 = mBody2->getTransform();
+    mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+    // Compute the initial orientation difference between the two bodies
+    mInitOrientationDifference = transform2.getOrientation() *
+                                 transform1.getOrientation().getInverse();
+    mInitOrientationDifference.normalize();
 
     // Compute the slider axis in local-space of body 1
     mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() *
@@ -54,11 +66,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
     mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
 
-    // Get the body positions
+    // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->getTransform().getPosition();
     const Vector3& x2 = mBody2->getTransform().getPosition();
-
-    // Get the bodies positions and orientations
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
@@ -74,23 +84,26 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     const Vector3 u = x2 + mR2 - x1 - mR1;
 
     // Compute the two orthogonal vectors to the slider axis in local-space of body 1
-    Vector3 sliderAxisWorld = orientationBody1 * mSliderAxisBody1;
+    mSliderAxisWorld = orientationBody1 * mSliderAxisBody1;
-    sliderAxisWorld.normalize();
+    mSliderAxisWorld.normalize();
-    mN1 = sliderAxisWorld.getOneUnitOrthogonalVector();
+    mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector();
-    mN2 = sliderAxisWorld.cross(mN1);
+    mN2 = mSliderAxisWorld.cross(mN1);
 
-    // Compute the cross products used in the Jacobian
+    // Check if the limit constraints are violated or not
+    decimal lowerLimitError = u.dot(mSliderAxisWorld) - mLowerLimit;
+    mIsLowerLimitViolated = (lowerLimitError <= 0);
+
+    // Compute the cross products used in the Jacobians
     mR2CrossN1 = mR2.cross(mN1);
     mR2CrossN2 = mR2.cross(mN2);
+    mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld);
     const Vector3 r1PlusU = mR1 + u;
     mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
     mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
+    mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld);
 
     // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
     // constraints (2x2 matrix)
-    const decimal n1Dotn1 = mN1.lengthSquare();
-    const decimal n2Dotn2 = mN2.lengthSquare();
-    const decimal n1Dotn2 = mN1.dot(mN2);
     decimal sumInverseMass = 0.0;
     Vector3 I1R1PlusUCrossN1(0, 0, 0);
     Vector3 I1R1PlusUCrossN2(0, 0, 0);
@@ -106,13 +119,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
         I2R2CrossN1 = I2 * mR2CrossN1;
         I2R2CrossN2 = I2 * mR2CrossN2;
     }
-    const decimal el11 = sumInverseMass * (n1Dotn1) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
+    const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
                          mR2CrossN1.dot(I2R2CrossN1);
-    const decimal el12 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
+    const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
                          mR2CrossN1.dot(I2R2CrossN2);
-    const decimal el21 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
+    const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
                          mR2CrossN2.dot(I2R2CrossN1);
-    const decimal el22 = sumInverseMass * (n2Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
+    const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
                          mR2CrossN2.dot(I2R2CrossN2);
     Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
     mInverseMassMatrixTranslationConstraint.setToZero();
@@ -120,6 +133,16 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
         mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
     }
 
+    // Compute the bias "b" of the translation constraint
+    mBTranslation.setToZero();
+    decimal beta = decimal(0.2);     // TODO : Use a constant here
+    decimal biasFactor = (beta / constraintSolverData.timeStep);
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        mBTranslation.x = u.dot(mN1);
+        mBTranslation.y = u.dot(mN2);
+        mBTranslation *= biasFactor;
+    }
+
     // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
     // contraints (3x3 matrix)
     mInverseMassMatrixRotationConstraint.setToZero();
@@ -132,6 +155,27 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
         mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
     }
+
+    // Compute the bias "b" of the rotation constraint
+    mBRotation.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
+        currentOrientationDifference.normalize();
+        const Quaternion qError = currentOrientationDifference *
+                                  mInitOrientationDifference.getInverse();
+        mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
+    }
+
+    // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix)
+    mInverseMassMatrixLowerLimit = sumInverseMass +
+                                   mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) +
+                                   mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis);
+
+    // Compute the bias "b" of the lower limit constraint
+    mBLowerLimit = 0.0;
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        mBLowerLimit = biasFactor * lowerLimitError;
+    }
 }
 
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
@@ -175,10 +219,6 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 // Solve the velocity constraint
 void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
-    // Get the body positions
-    const Vector3& x1 = mBody1->getTransform().getPosition();
-    const Vector3& x2 = mBody2->getTransform().getPosition();
-
     // Get the velocities
     Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
     Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
@@ -200,18 +240,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
                          mN2.dot(v2) + w2.dot(mR2CrossN2);
     const Vector2 JvTranslation(el1, el2);
 
-    // Compute the bias "b" of the translation constraint
-    Vector2 bTranslation(0, 0);
-    decimal beta = decimal(0.2);     // TODO : Use a constant here
-    decimal biasFactor = (beta / constraintSolverData.timeStep);
-    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
-        Vector3 deltaV = x2 + mR2 - x1 - mR1;
-        bTranslation.x = biasFactor * deltaV.dot(mN1);
-        bTranslation.y = biasFactor * deltaV.dot(mN2);
-    }
-
     // Compute the Lagrange multiplier lambda for the 2 translation constraints
-    Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation);
+    Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
     mImpulseTranslation += deltaLambda;
 
     // Compute the impulse P=J^T * lambda for the 2 translation constraints
@@ -235,17 +265,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
     // Compute J*v for the 3 rotation constraints
     const Vector3 JvRotation = w2 - w1;
 
-    // Compute the bias "b" of the rotation constraint
-    Vector3 bRotation(0, 0, 0);
-    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
-        Quaternion q1 = mBody1->getTransform().getOrientation();
-        Quaternion q2 = mBody2->getTransform().getOrientation();
-        Quaternion qDiff = q1 * q2.getInverse();
-        bRotation = 2.0 * qDiff.getVectorV();
-    }
-
     // Compute the Lagrange multiplier lambda for the 3 rotation constraints
-    Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation);
+    Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
     mImpulseRotation += deltaLambda2;
 
     // Compute the impulse P=J^T * lambda for the 3 rotation constraints
@@ -259,6 +280,41 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
     if (mBody2->getIsMotionEnabled()) {
         w2 += I2 * angularImpulseBody2;
     }
+
+    // --------------- Limits Constraints --------------- //
+
+    if (mIsLimitsActive) {
+
+        // If the lower limit is violated
+        if (mIsLowerLimitViolated) {
+
+            // Compute J*v for the lower limit constraint
+            const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) -
+                                         mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1);
+
+            // Compute the Lagrange multiplier lambda for the lower limit constraint
+            decimal deltaLambdaLower = mInverseMassMatrixLowerLimit * (-JvLowerLimit -mBLowerLimit);
+            decimal lambdaTemp = mImpulseLowerLimit;
+            mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
+            deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
+
+            // Compute the impulse P=J^T * lambda for the lower limit constraint
+            Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
+            Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
+            Vector3 linearImpulseBody2 = -linearImpulseBody1;
+            Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
+
+            // Apply the impulse to the bodies of the joint
+            if (mBody1->getIsMotionEnabled()) {
+                v1 += inverseMassBody1 * linearImpulseBody1;
+                w1 += I1 * angularImpulseBody1;
+            }
+            if (mBody2->getIsMotionEnabled()) {
+                v2 += inverseMassBody2 * linearImpulseBody2;
+                w2 += I2 * angularImpulseBody2;
+            }
+        }
+    }
 }
 
 // Solve the position constraint

src/constraint/SliderJoint.h

@@ -49,13 +49,34 @@ struct SliderJointInfo : public ConstraintInfo {
         /// Slider axis (in world-space coordinates)
         Vector3 sliderAxisWorldSpace;
 
-        /// Constructor
+        /// True if the slider limits are active
+        bool isLimitsActive;
+
+        /// Lower limit
+        decimal lowerLimit;
+
+        /// Upper limit
+        decimal upperLimit;
+
+        /// Constructor without limits
         SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
                         const Vector3& initAnchorPointWorldSpace,
                         const Vector3& initSliderAxisWorldSpace)
                        : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
                          anchorPointWorldSpace(initAnchorPointWorldSpace),
-                         sliderAxisWorldSpace(initSliderAxisWorldSpace) {}
+                         sliderAxisWorldSpace(initSliderAxisWorldSpace),
+                         isLimitsActive(false), lowerLimit(-1.0), upperLimit(1.0) {}
+
+        /// Constructor with limits
+        SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                        const Vector3& initAnchorPointWorldSpace,
+                        const Vector3& initSliderAxisWorldSpace,
+                        decimal initLowerLimit, decimal initUpperLimit)
+                       : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
+                         anchorPointWorldSpace(initAnchorPointWorldSpace),
+                         sliderAxisWorldSpace(initSliderAxisWorldSpace),
+                         isLimitsActive(true), lowerLimit(initLowerLimit),
+                         upperLimit(initUpperLimit) {}
 };
 
 // Class SliderJoint
@@ -77,6 +98,9 @@ class SliderJoint : public Constraint {
         /// Slider axis (in local-space coordinates of body 1)
         Vector3 mSliderAxisBody1;
 
+        /// Initial orientation difference between the two bodies
+        Quaternion mInitOrientationDifference;
+
         /// First vector orthogonal to the slider axis local-space of body 1
         Vector3 mN1;
 
@@ -95,24 +119,63 @@ class SliderJoint : public Constraint {
         /// Cross product of r2 and n2
         Vector3 mR2CrossN2;
 
+        /// Cross product of r2 and the slider axis
+        Vector3 mR2CrossSliderAxis;
+
         /// Cross product of vector (r1 + u) and n1
         Vector3 mR1PlusUCrossN1;
 
         /// Cross product of vector (r1 + u) and n2
         Vector3 mR1PlusUCrossN2;
 
+        /// Cross product of vector (r1 + u) and the slider axis
+        Vector3 mR1PlusUCrossSliderAxis;
+
+        /// Bias of the 2 translation constraints
+        Vector2 mBTranslation;
+
+        /// Bias of the 3 rotation constraints
+        Vector3 mBRotation;
+
+        /// Bias of the lower limit constraint
+        decimal mBLowerLimit;
+
         /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
         Matrix2x2 mInverseMassMatrixTranslationConstraint;
 
         /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix)
         Matrix3x3 mInverseMassMatrixRotationConstraint;
 
+        /// Inverse of mass matrix K=JM^-1J^t for the lower limit constraint (1x1 matrix)
+        decimal mInverseMassMatrixLowerLimit;
+
         /// Impulse for the 2 translation constraints
         Vector2 mImpulseTranslation;
 
         /// Impulse for the 3 rotation constraints
         Vector3 mImpulseRotation;
 
+        /// Impulse for the lower limit constraint
+        decimal mImpulseLowerLimit;
+
+        /// True if the slider limits are active
+        bool mIsLimitsActive;
+
+        /// Slider axis in world-space coordinates
+        Vector3 mSliderAxisWorld;
+
+        /// Lower limit
+        decimal mLowerLimit;
+
+        /// Upper limit
+        decimal mUpperLimit;
+
+        /// True if the lower limit is violated
+        bool mIsLowerLimitViolated;
+
+        /// True if the upper limit is violated
+        bool mIsUpperLimitViolated;
+
     public :
 
         // -------------------- Methods -------------------- //