continue working on the slider joint

src/constraint/BallAndSocketJoint.cpp

@@ -78,6 +78,38 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     mInverseMassMatrix = massMatrix.getInverse();
 }
 
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    decimal inverseMassBody1 = mBody1->getMassInverse();
+    decimal inverseMassBody2 = mBody2->getMassInverse();
+    Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the impulse P=J^T * lambda
+    Vector3 linearImpulseBody1 = -mImpulse;
+    Vector3 angularImpulseBody1 = mImpulse.cross(mU1World);
+    Vector3 linearImpulseBody2 = mImpulse;
+    Vector3 angularImpulseBody2 = -mImpulse.cross(mU2World);
+
+    // 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 velocity constraint
 void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
@@ -94,8 +126,8 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
     // Get the inverse mass and inverse inertia tensors of the bodies
     decimal inverseMassBody1 = mBody1->getMassInverse();
     decimal inverseMassBody2 = mBody2->getMassInverse();
-    Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
-    Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
+    Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
     // Compute J*v
     Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2);
@@ -121,11 +153,11 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
     // Apply the impulse to the bodies of the joint
     if (mBody1->getIsMotionEnabled()) {
         v1 += inverseMassBody1 * linearImpulseBody1;
-        w1 += inverseInertiaTensorBody1 * angularImpulseBody1;
+        w1 += I1 * angularImpulseBody1;
     }
     if (mBody2->getIsMotionEnabled()) {
         v2 += inverseMassBody2 * linearImpulseBody2;
-        w2 += inverseInertiaTensorBody2 * angularImpulseBody2;
+        w2 += I2 * angularImpulseBody2;
     }
 }
 

src/constraint/BallAndSocketJoint.h

@@ -104,6 +104,9 @@ class BallAndSocketJoint : public Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
 
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData);
+
         /// Solve the velocity constraint
         virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
 

src/constraint/Constraint.h

@@ -149,6 +149,9 @@ class Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
 
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0;
+
         /// Solve the velocity constraint
         virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
 

src/constraint/ContactPoint.cpp

@@ -56,6 +56,11 @@ void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverD
 
 }
 
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void ContactPoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+}
+
 // Solve the velocity constraint
 void ContactPoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 

src/constraint/ContactPoint.h

@@ -227,6 +227,9 @@ class ContactPoint : public Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
 
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData);
+
         /// Solve the velocity constraint
         virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
 

src/constraint/SliderJoint.cpp

@@ -29,7 +29,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo) {
+SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
+            : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0) {
 
     // Compute the local-space anchor point for each body
     mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
@@ -64,17 +65,76 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     mN1 = mU1World.getOneUnitOrthogonalVector();
     mN2 = mU1World.cross(mN1);
 
-    // Compute the cross product used in the Jacobian
+    // Compute the cross products used in the Jacobian
     mU1WorldCrossN1 = mN2;
     mU1WorldCrossN2 = mU1World.cross(mN2);
     mU2WorldCrossN1 = mU2World.cross(mN1);
     mU2WorldCrossN2 = mU2World.cross(mN2);
 
-    // Compute the mass matrix K=JM^-1J^t for the 2 translation constraints (2x2 matrix)
+    // 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);
     const decimal sumInverseMass = mBody1->getMassInverse() + mBody2->getMassInverse();
+    const Vector3 I1U2CrossN1 = I1 * mU2WorldCrossN1;
+    const Vector3 I1U2CrossN2 = I1 * mU2WorldCrossN2;
+    const Vector3 I2U1CrossN1 = I2 * mU1WorldCrossN1;
+    const Vector3 I2U1CrossN2 = I2 * mU1WorldCrossN2;
+    const decimal el11 = sumInverseMass * (n1Dotn1) + mU2WorldCrossN1.dot(I1U2CrossN1) +
+                         mU1WorldCrossN1.dot(I2U1CrossN1);
+    const decimal el12 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN1.dot(I1U2CrossN2) +
+            mU1WorldCrossN1.dot(I2U1CrossN2);
+    const decimal el21 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN1) +
+            mU1WorldCrossN2.dot(I2U1CrossN1);
+    const decimal el22 = sumInverseMass * (n2Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN2) +
+            mU1WorldCrossN2.dot(I2U1CrossN2);
+    Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
+    mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
 
+    // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+    // contraints (3x3 matrix)
+    mInverseMassMatrixRotationConstraint = I1 + I2;
+}
+
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    decimal inverseMassBody1 = mBody1->getMassInverse();
+    decimal inverseMassBody2 = mBody2->getMassInverse();
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the impulse P=J^T * lambda for the 2 translation constraints
+    Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x -
+                                  mN2 * mImpulseTranslation.y;
+    Vector3 angularImpulseBody1 = mU2WorldCrossN1 * mImpulseTranslation.x +
+                                  mU2WorldCrossN2 * mImpulseTranslation.y;
+    Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x +
+                                 mN2 * mImpulseTranslation.y;
+    Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * mImpulseTranslation.x -
+                                   mU1WorldCrossN2 * mImpulseTranslation.y;
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints
+    angularImpulseBody1 += -mImpulseRotation;
+    angularImpulseBody2 += mImpulseRotation;
+
+    // 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 velocity constraint
@@ -93,10 +153,65 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
     // Get the inverse mass and inverse inertia tensors of the bodies
     decimal inverseMassBody1 = mBody1->getMassInverse();
     decimal inverseMassBody2 = mBody2->getMassInverse();
-    Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
-    Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
-    // Compute J*v
+    // Compute J*v for the 2 translation constraints
+    const decimal el1 = -mN1.dot(v1) + mU2WorldCrossN1.dot(w1) +
+                         mN1.dot(v2) - mU1WorldCrossN1.dot(w2);
+    const decimal el2 = -mN2.dot(v1) + mU2WorldCrossN2.dot(w1) +
+                         mN2.dot(v2) - mU1WorldCrossN2.dot(w2);
+    const Vector2 JvTranslation(el1, el2);
+
+    // Compute J*v for the 3 translation constraints
+    const Vector3 JvRotation = w2 - w1;
+
+    // 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 + mU2World - x1 - mU1World;
+        bTranslation.x = biasFactor * deltaV.dot(mN1);
+        bTranslation.y = biasFactor * deltaV.dot(mN2);
+    }
+
+    // Compute the bias "b" of the translation 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 2 translation constraints
+    Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation);
+    mImpulseTranslation += deltaLambda;
+
+    // Compute the Lagrange multiplier lambda for the 3 rotation constraints
+    Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation);
+    mImpulseRotation += deltaLambda2;
+
+    // Compute the impulse P=J^T * lambda for the 2 translation constraints
+    Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
+    Vector3 angularImpulseBody1 = mU2WorldCrossN1 * deltaLambda.x + mU2WorldCrossN2 * deltaLambda.y;
+    Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
+    Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * deltaLambda.x -mU1WorldCrossN2 * deltaLambda.y;
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints
+    angularImpulseBody1 += -deltaLambda2;
+    angularImpulseBody2 += deltaLambda2;
+
+    // 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

@@ -98,6 +98,18 @@ class SliderJoint : public Constraint {
         /// Cross product of mU2World and mN2
         Vector3 mU2WorldCrossN2;
 
+        /// 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;
+
+        /// Impulse for the 2 translation constraints
+        Vector2 mImpulseTranslation;
+
+        /// Impulse for the 3 rotation constraints
+        Vector3 mImpulseRotation;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -114,6 +126,9 @@ class SliderJoint : public Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
 
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData);
+
         /// Solve the velocity constraint
         virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
 

src/mathematics/mathematics.h

@@ -28,8 +28,10 @@
 
 // Libraries
 #include "Matrix3x3.h"
+#include "Matrix2x2.h"
 #include "Quaternion.h"
 #include "Vector3.h"
+#include "Vector2.h"
 #include "Transform.h"
 #include "../configuration.h"
 #include "mathematics_functions.h"