Fix issues in the BallAndSocketJoint and SliderJoint

examples/joints/Scene.cpp

@@ -59,6 +59,9 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
     // Create the Ball-and-Socket joint
     createBallAndSocketJoints();
 
+    // Create the Slider joint
+    createSliderJoint();
+
     // Create the floor
     createFloor();
 
@@ -77,12 +80,17 @@ Scene::~Scene() {
 
     // Destroy the joints
     mDynamicsWorld->destroyJoint(mBallAndSocketJoint);
+    mDynamicsWorld->destroyJoint(mSliderJoint);
 
     // Destroy all the boxes of the scene
     mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox1->getRigidBody());
     mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox2->getRigidBody());
+    mDynamicsWorld->destroyRigidBody(mSliderJointBox1->getRigidBody());
+    mDynamicsWorld->destroyRigidBody(mSliderJointBox2->getRigidBody());
     delete mBallAndSocketJointBox1;
     delete mBallAndSocketJointBox2;
+    delete mSliderJointBox1;
+    delete mSliderJointBox2;
 
     // Destroy the floor
     mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
@@ -104,10 +112,11 @@ void Scene::simulate() {
         // Update the position and orientation of the boxes
         mBallAndSocketJointBox1->updateTransform();
         mBallAndSocketJointBox2->updateTransform();
+        mSliderJointBox1->updateTransform();
+        mSliderJointBox2->updateTransform();
 
         // Update the position and orientation of the floor
         mFloor->updateTransform();
-
     }
 }
 
@@ -139,6 +148,8 @@ void Scene::render() {
     // Render all the boxes
     mBallAndSocketJointBox1->render(mPhongShader);
     mBallAndSocketJointBox2->render(mPhongShader);
+    mSliderJointBox1->render(mPhongShader);
+    mSliderJointBox2->render(mPhongShader);
 
     // Render the floor
     mFloor->render(mPhongShader);
@@ -191,6 +202,52 @@ void Scene::createBallAndSocketJoints() {
                                                   mDynamicsWorld->createJoint(jointInfo));
 }
 
+/// Create the boxes and joint for the Slider joint example
+void Scene::createSliderJoint() {
+
+    // --------------- Create the first box --------------- //
+
+    // Position of the box
+    openglframework::Vector3 positionBox1(-4, 6, 0);
+
+    // Create a box and a corresponding rigid in the dynamics world
+    mSliderJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld);
+
+    // The fist box cannot move
+    mSliderJointBox1->getRigidBody()->setIsMotionEnabled(false);
+
+    // Set the bouncing factor of the box
+    mSliderJointBox1->getRigidBody()->setRestitution(0.4);
+
+    // --------------- Create the second box --------------- //
+
+    // Position of the box
+    openglframework::Vector3 positionBox2(2, 4, 0);
+
+    // Create a box and a corresponding rigid in the dynamics world
+    mSliderJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld);
+
+    // The second box is allowed to move
+    mSliderJointBox2->getRigidBody()->setIsMotionEnabled(true);
+
+    // Set the bouncing factor of the box
+    mSliderJointBox2->getRigidBody()->setRestitution(0.4);
+
+    // --------------- Create the joint --------------- //
+
+    // Create the joint info object
+    rp3d::RigidBody* body1 = mSliderJointBox1->getRigidBody();
+    rp3d::RigidBody* body2 = mSliderJointBox2->getRigidBody();
+    const rp3d::Vector3& body1Position = body1->getTransform().getPosition();
+    const rp3d::Vector3& body2Position = body2->getTransform().getPosition();
+    const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position);
+    const rp3d::Vector3 sliderAxisWorldSpace = body2Position - body1Position;
+    rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace);
+
+    // Create the joint in the dynamics world
+    mSliderJoint = dynamic_cast<rp3d::SliderJoint*>(mDynamicsWorld->createJoint(jointInfo));
+}
+
 // Create the floor
 void Scene::createFloor() {
 

examples/joints/Scene.h

@@ -62,6 +62,15 @@ class Scene {
         /// Ball-and-Socket joint
         rp3d::BallAndSocketJoint* mBallAndSocketJoint;
 
+        /// Box 1 of Slider joint
+        Box* mSliderJointBox1;
+
+        /// Box 2 of Slider joint
+        Box* mSliderJointBox2;
+
+        /// Slider joint
+        rp3d::SliderJoint* mSliderJoint;
+
         /// Box for the floor
         Box* mFloor;
 
@@ -76,6 +85,9 @@ class Scene {
         /// Create the boxes and joints for the Ball-and-Socket joint example
         void createBallAndSocketJoints();
 
+        /// Create the boxes and joint for the Slider joint example
+        void createSliderJoint();
+
         /// Create the floor
         void createFloor();
 

src/constraint/BallAndSocketJoint.cpp

@@ -59,23 +59,36 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
     // Compute the vector from body center to the anchor point in world-space
-    mU1World = orientationBody1 * mLocalAnchorPointBody1;
+    mR1World = orientationBody1 * mLocalAnchorPointBody1;
-    mU2World = orientationBody2 * mLocalAnchorPointBody2;
+    mR2World = orientationBody2 * mLocalAnchorPointBody2;
 
     // Compute the corresponding skew-symmetric matrices
-    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World);
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
-    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
 
     // Compute the matrix K=JM^-1J^t (3x3 matrix)
-    decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse();
+    decimal inverseMassBodies = 0.0;
-    Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0,
+    if (mBody1->getIsMotionEnabled()) {
+        inverseMassBodies += mBody1->getMassInverse();
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        inverseMassBodies += mBody2->getMassInverse();
+    }
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
-                                    0, 0, inverseMassBodies) +
+                                    0, 0, inverseMassBodies);
-                          skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose() +
+    if (mBody1->getIsMotionEnabled()) {
-                          skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
+        massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose();
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
+    }
 
     // Compute the inverse mass matrix K^-1
-    mInverseMassMatrix = massMatrix.getInverse();
+    mInverseMassMatrix.setToZero();
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrix = massMatrix.getInverse();
+    }
 }
 
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
@@ -88,16 +101,16 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
     Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->getMassInverse();
+    const decimal inverseMassBody1 = mBody1->getMassInverse();
-    decimal inverseMassBody2 = mBody2->getMassInverse();
+    const decimal inverseMassBody2 = mBody2->getMassInverse();
-    Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
-    Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
     // Compute the impulse P=J^T * lambda
-    Vector3 linearImpulseBody1 = -mImpulse;
+    const Vector3 linearImpulseBody1 = -mImpulse;
-    Vector3 angularImpulseBody1 = mImpulse.cross(mU1World);
+    const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
-    Vector3 linearImpulseBody2 = mImpulse;
+    const Vector3 linearImpulseBody2 = mImpulse;
-    Vector3 angularImpulseBody2 = -mImpulse.cross(mU2World);
+    const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
 
     // Apply the impulse to the bodies of the joint
     if (mBody1->getIsMotionEnabled()) {
@@ -130,25 +143,25 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
     Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
     // Compute J*v
-    Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2);
+    const Vector3 Jv = -v1 + mR1World.cross(w1) + v2 - mR2World.cross(w2);
 
     // Compute the bias "b" of the constraint
     Vector3 b(0, 0, 0);
     if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
         decimal beta = decimal(0.2);     // TODO : Use a constant here
         decimal biasFactor = (beta / constraintSolverData.timeStep);
-        b = biasFactor * (x2 + mU2World - x1 - mU1World);
+        b = biasFactor * (x2 + mR2World - x1 - mR1World);
     }
 
     // Compute the Lagrange multiplier lambda
-    Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b);
+    const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b);
     mImpulse += deltaLambda;
 
     // Compute the impulse P=J^T * lambda
-    Vector3 linearImpulseBody1 = -deltaLambda;
+    const Vector3 linearImpulseBody1 = -deltaLambda;
-    Vector3 angularImpulseBody1 = deltaLambda.cross(mU1World);
+    const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
-    Vector3 linearImpulseBody2 = deltaLambda;
+    const Vector3 linearImpulseBody2 = deltaLambda;
-    Vector3 angularImpulseBody2 = -deltaLambda.cross(mU2World);
+    const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
 
     // Apply the impulse to the bodies of the joint
     if (mBody1->getIsMotionEnabled()) {

src/constraint/BallAndSocketJoint.h

@@ -50,7 +50,7 @@ struct BallAndSocketJointInfo : public ConstraintInfo {
         BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
                                const Vector3& initAnchorPointWorldSpace)
                               : ConstraintInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
-                                anchorPointWorldSpace(initAnchorPointWorldSpace){}
+                                anchorPointWorldSpace(initAnchorPointWorldSpace) {}
 };
 
 // Class BallAndSocketJoint
@@ -64,23 +64,23 @@ class BallAndSocketJoint : public Constraint {
 
         // -------------------- Attributes -------------------- //
 
-        /// Anchor point of body 1 (in local space coordinates)
+        /// Anchor point of body 1 (in local-space coordinates of body 1)
         Vector3 mLocalAnchorPointBody1;
 
-        /// Anchor point of body 2 (in local space coordinates)
+        /// Anchor point of body 2 (in local-space coordinates of body 2)
         Vector3 mLocalAnchorPointBody2;
 
         /// Vector from center of body 2 to anchor point in world-space
-        Vector3 mU1World;
+        Vector3 mR1World;
 
         /// Vector from center of body 2 to anchor point in world-space
-        Vector3 mU2World;
+        Vector3 mR2World;
 
-        /// Skew-Symmetric matrix for cross product with vector mU1World
+        /// Skew-Symmetric matrix for cross product with vector mR1World
-        Matrix3x3 mSkewSymmetricMatrixU1World;
+        Matrix3x3 mSkewSymmetricMatrixR1World;
 
-        /// Skew-Symmetric matrix for cross product with vector mU2World
+        /// Skew-Symmetric matrix for cross product with vector mR2World
-        Matrix3x3 mSkewSymmetricMatrixU2World;
+        Matrix3x3 mSkewSymmetricMatrixR2World;
 
         /// Inverse mass matrix K=JM^-1J^-t of the constraint
         Matrix3x3 mInverseMassMatrix;

src/constraint/SliderJoint.cpp

@@ -35,6 +35,11 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
     // Compute the local-space anchor point for each body
     mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
     mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+
+    // Compute the slider axis in local-space of body 1
+    mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() *
+                       jointInfo.sliderAxisWorldSpace;
+    mSliderAxisBody1.normalize();
 }
 
 // Destructor
@@ -49,6 +54,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
     mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
 
+    // Get the body positions
+    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();
@@ -57,44 +66,72 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
     const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
-    // Compute the vector from body center to the anchor point in world-space
+    // Vector from body center to the anchor point
-    mU1World = orientationBody1 * mLocalAnchorPointBody1;
+    mR1 = orientationBody1 * mLocalAnchorPointBody1;
-    mU2World = orientationBody2 * mLocalAnchorPointBody2;
+    mR2 = orientationBody2 * mLocalAnchorPointBody2;
 
-    // Compute the two orthogonal vectors to vector mU1World in world-space
+    // Compute the vector u
-    mN1 = mU1World.getOneUnitOrthogonalVector();
+    const Vector3 u = x2 + mR2 - x1 - mR1;
-    mN2 = mU1World.cross(mN1);
+
+    // Compute the two orthogonal vectors to the slider axis in local-space of body 1
+    Vector3 sliderAxisWorld = orientationBody1 * mSliderAxisBody1;
+    sliderAxisWorld.normalize();
+    mN1 = sliderAxisWorld.getOneUnitOrthogonalVector();
+    mN2 = sliderAxisWorld.cross(mN1);
 
     // Compute the cross products used in the Jacobian
-    mU1WorldCrossN1 = mN2;
+    mR2CrossN1 = mR2.cross(mN1);
-    mU1WorldCrossN2 = mU1World.cross(mN2);
+    mR2CrossN2 = mR2.cross(mN2);
-    mU2WorldCrossN1 = mU2World.cross(mN1);
+    const Vector3 r1PlusU = mR1 + u;
-    mU2WorldCrossN2 = mU2World.cross(mN2);
+    mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
+    mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
 
     // 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();
+    decimal sumInverseMass = 0.0;
-    const Vector3 I1U2CrossN1 = I1 * mU2WorldCrossN1;
+    Vector3 I1R1PlusUCrossN1(0, 0, 0);
-    const Vector3 I1U2CrossN2 = I1 * mU2WorldCrossN2;
+    Vector3 I1R1PlusUCrossN2(0, 0, 0);
-    const Vector3 I2U1CrossN1 = I2 * mU1WorldCrossN1;
+    Vector3 I2R2CrossN1(0, 0, 0);
-    const Vector3 I2U1CrossN2 = I2 * mU1WorldCrossN2;
+    Vector3 I2R2CrossN2(0, 0, 0);
-    const decimal el11 = sumInverseMass * (n1Dotn1) + mU2WorldCrossN1.dot(I1U2CrossN1) +
+    if (mBody1->getIsMotionEnabled()) {
-                         mU1WorldCrossN1.dot(I2U1CrossN1);
+        sumInverseMass += mBody1->getMassInverse();
-    const decimal el12 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN1.dot(I1U2CrossN2) +
+        I1R1PlusUCrossN1 = I1 * mR1PlusUCrossN1;
-            mU1WorldCrossN1.dot(I2U1CrossN2);
+        I1R1PlusUCrossN2 = I1 * mR1PlusUCrossN2;
-    const decimal el21 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN1) +
+    }
-            mU1WorldCrossN2.dot(I2U1CrossN1);
+    if (mBody2->getIsMotionEnabled()) {
-    const decimal el22 = sumInverseMass * (n2Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN2) +
+        sumInverseMass += mBody2->getMassInverse();
-            mU1WorldCrossN2.dot(I2U1CrossN2);
+        I2R2CrossN1 = I2 * mR2CrossN1;
+        I2R2CrossN2 = I2 * mR2CrossN2;
+    }
+    const decimal el11 = sumInverseMass * (n1Dotn1) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
+                         mR2CrossN1.dot(I2R2CrossN1);
+    const decimal el12 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
+                         mR2CrossN1.dot(I2R2CrossN2);
+    const decimal el21 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
+                         mR2CrossN2.dot(I2R2CrossN1);
+    const decimal el22 = sumInverseMass * (n2Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
+                         mR2CrossN2.dot(I2R2CrossN2);
     Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
-    mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
+    mInverseMassMatrixTranslationConstraint.setToZero();
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
+    }
 
     // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
     // contraints (3x3 matrix)
-    mInverseMassMatrixRotationConstraint = I1 + I2;
+    mInverseMassMatrixRotationConstraint.setToZero();
+    if (mBody1->getIsMotionEnabled()) {
+        mInverseMassMatrixRotationConstraint += I1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixRotationConstraint += I2;
+    }
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
+    }
 }
 
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
@@ -107,20 +144,18 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
     Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->getMassInverse();
+    const decimal inverseMassBody1 = mBody1->getMassInverse();
-    decimal inverseMassBody2 = mBody2->getMassInverse();
+    const 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 -
+    Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
-                                  mN2 * mImpulseTranslation.y;
+    Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
-    Vector3 angularImpulseBody1 = mU2WorldCrossN1 * mImpulseTranslation.x +
+                                   mR1PlusUCrossN2 * mImpulseTranslation.y;
-                                  mU2WorldCrossN2 * mImpulseTranslation.y;
+    Vector3 linearImpulseBody2 = -linearImpulseBody1;
-    Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x +
+    Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
-                                 mN2 * mImpulseTranslation.y;
+                                  mR2CrossN2 * mImpulseTranslation.y;
-    Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * mImpulseTranslation.x -
-                                   mU1WorldCrossN2 * mImpulseTranslation.y;
 
     // Compute the impulse P=J^T * lambda for the 3 rotation constraints
     angularImpulseBody1 += -mImpulseRotation;
@@ -156,52 +191,34 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
     const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
     const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
-    // Compute J*v for the 2 translation constraints
+    // --------------- 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
+    // Compute J*v for the 2 translation constraints
-    const Vector3 JvRotation = w2 - w1;
+    const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) +
+                         mN1.dot(v2) + w2.dot(mR2CrossN1);
+    const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) +
+                         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 + mU2World - x1 - mU1World;
+        Vector3 deltaV = x2 + mR2 - x1 - mR1;
         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 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -mR1PlusUCrossN2 * deltaLambda.y;
-    Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
+    Vector3 linearImpulseBody2 = -linearImpulseBody1;
-    Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * deltaLambda.x -mU1WorldCrossN2 * deltaLambda.y;
+    Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * 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()) {
@@ -212,6 +229,36 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
         v2 += inverseMassBody2 * linearImpulseBody2;
         w2 += I2 * angularImpulseBody2;
     }
+
+    // --------------- Rotation Constraints --------------- //
+
+    // 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);
+    mImpulseRotation += deltaLambda2;
+
+    // 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()) {
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        w2 += I2 * angularImpulseBody2;
+    }
 }
 
 // Solve the position constraint

src/constraint/SliderJoint.h

@@ -47,15 +47,15 @@ struct SliderJointInfo : public ConstraintInfo {
         Vector3 anchorPointWorldSpace;
 
         /// Slider axis (in world-space coordinates)
-        Vector3 axisWorldSpace;
+        Vector3 sliderAxisWorldSpace;
 
         /// Constructor
         SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
                         const Vector3& initAnchorPointWorldSpace,
-                        const Vector3& initAxisWorldSpace)
+                        const Vector3& initSliderAxisWorldSpace)
                        : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
                          anchorPointWorldSpace(initAnchorPointWorldSpace),
-                         axisWorldSpace(initAxisWorldSpace) {}
+                         sliderAxisWorldSpace(initSliderAxisWorldSpace) {}
 };
 
 // Class SliderJoint
@@ -68,35 +68,38 @@ class SliderJoint : public Constraint {
 
         // -------------------- Attributes -------------------- //
 
-        /// Anchor point of body 1 (in local space coordinates)
+        /// Anchor point of body 1 (in local-space coordinates of body 1)
         Vector3 mLocalAnchorPointBody1;
 
-        /// Anchor point of body 2 (in local space coordinates)
+        /// Anchor point of body 2 (in local-space coordinates of body 2)
         Vector3 mLocalAnchorPointBody2;
 
-        /// Vector from center of body 2 to anchor point in world-space
+        /// Slider axis (in local-space coordinates of body 1)
-        Vector3 mU1World;
+        Vector3 mSliderAxisBody1;
 
-        /// Vector from center of body 2 to anchor point in world-space
+        /// First vector orthogonal to the slider axis local-space of body 1
-        Vector3 mU2World;
-
-        /// First vector orthogonal to vector mU1World in world-space
         Vector3 mN1;
 
-        /// Second vector orthogonal to vector mU1World and mN1 in world-space
+        /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
         Vector3 mN2;
 
-        /// Cross product of mU1World and mN1
+        /// Vector r1 in world-space coordinates
-        Vector3 mU1WorldCrossN1;
+        Vector3 mR1;
 
-        /// Cross product of mU1World and mN2
+        /// Vector r2 in world-space coordinates
-        Vector3 mU1WorldCrossN2;
+        Vector3 mR2;
 
-        /// Cross product of mU2World and mN1
+        /// Cross product of r2 and n1
-        Vector3 mU2WorldCrossN1;
+        Vector3 mR2CrossN1;
 
-        /// Cross product of mU2World and mN2
+        /// Cross product of r2 and n2
-        Vector3 mU2WorldCrossN2;
+        Vector3 mR2CrossN2;
+
+        /// Cross product of vector (r1 + u) and n1
+        Vector3 mR1PlusUCrossN1;
+
+        /// Cross product of vector (r1 + u) and n2
+        Vector3 mR1PlusUCrossN2;
 
         /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
         Matrix2x2 mInverseMassMatrixTranslationConstraint;

src/mathematics/Matrix2x2.h

@@ -44,7 +44,7 @@ class Matrix2x2 {
         // -------------------- Attributes -------------------- //
 
         /// Rows of the matrix;
-        Vector2 mRows[3];
+        Vector2 mRows[2];
 
     public :
 

src/reactphysics3d.h

@@ -48,6 +48,7 @@
 #include "collision/shapes/CylinderShape.h"
 #include "collision/shapes/AABB.h"
 #include "constraint/BallAndSocketJoint.h"
+#include "constraint/SliderJoint.h"
 
 /// Alias to the ReactPhysics3D namespace
 namespace rp3d = reactphysics3d;