Start working on the slider joint

examples/fallingcubes/Scene.cpp

@@ -54,7 +54,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
     mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep);
 
     // Set the number of iterations of the constraint solver
-    mDynamicsWorld->setNbIterationsSolver(15);
+    mDynamicsWorld->setNbIterationsVelocitySolver(15);
 
     float radius = 2.0f;
 

examples/joints/Scene.cpp

@@ -54,7 +54,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
     mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep);
 
     // Set the number of iterations of the constraint solver
-    mDynamicsWorld->setNbIterationsSolver(15);
+    mDynamicsWorld->setNbIterationsVelocitySolver(15);
 
     // Create the Ball-and-Socket joint
     createBallAndSocketJoints();
@@ -181,10 +181,10 @@ void Scene::createBallAndSocketJoints() {
     // --------------- Create the joint --------------- //
 
     // Create the joint info object
-    rp3d::BallAndSocketJointInfo jointInfo;
+    rp3d::RigidBody* body1 = mBallAndSocketJointBox1->getRigidBody();
-    jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody();
+    rp3d::RigidBody* body2 = mBallAndSocketJointBox2->getRigidBody();
-    jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody();
+    const rp3d::Vector3 anchorPointWorldSpace(0, 10, 0);
-    jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 10, 0);
+    rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace);
 
     // Create the joint in the dynamics world
     mBallAndSocketJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(

src/configuration.h

@@ -51,6 +51,22 @@ typedef long unsigned int luint;
 typedef luint bodyindex;
 typedef std::pair<bodyindex, bodyindex> bodyindexpair;
 
+// ------------------- Enumerations ------------------- //
+
+/// Position correction technique used in the constraint solver (for joints).
+/// BAUMGARTE : Faster but can be innacurate in some situations. This is the option
+///             used by default.
+/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise.
+enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
+
+/// Position correction technique used in the contact solver (for contacts)
+/// BAUMGARTE : Faster but can be innacurate and can lead to unexpected bounciness
+///             in some situations (due to error correction factor being added to
+///             the bodies momentum).
+/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
+///                 bodies momentum. This is the option used by default.
+enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
+
 // ------------------- Constants ------------------- //
 
 /// Smallest decimal value (negative)
@@ -83,8 +99,11 @@ const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
 /// Velocity threshold for contact velocity restitution
 const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
 
-/// Number of iterations when solving a LCP problem
+/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
-const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15;
+const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15;
+
+/// Number of iterations when solving the position constraints of the Sequential Impulse technique
+const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 3;  // TODO : Maybe we can use less iterations here
 
 }
 

src/constraint/BallAndSocketJoint.cpp

@@ -55,8 +55,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
     // Get the inertia tensor of bodies
-    Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
-    Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
 
     // Compute the vector from body center to the anchor point in world-space
     mU1World = orientationBody1 * mLocalAnchorPointBody1;
@@ -66,22 +66,20 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World);
     Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World);
 
-    // Compute the matrix JM^-1J^t
+    // Compute the matrix K=JM^-1J^t (3x3 matrix)
     decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse();
     Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
-                          skewSymmetricMatrixU1 * inverseInertiaTensorBody1 *
+                          skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose() +
-                          skewSymmetricMatrixU1.getTranspose() +
+                          skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
-                          skewSymmetricMatrixU2 * inverseInertiaTensorBody2 *
-                          skewSymmetricMatrixU2.getTranspose();
 
-    // Compute the inverse mass matrix K
+    // Compute the inverse mass matrix K^-1
     mInverseMassMatrix = massMatrix.getInverse();
 }
 
-// Solve the constraint
+// Solve the velocity constraint
-void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) {
+void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
     // Get the body positions
     const Vector3& x1 = mBody1->getTransform().getPosition();
@@ -103,9 +101,12 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData)
     Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.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);
-    Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World);
+        b = biasFactor * (x2 + mU2World - x1 - mU1World);
+    }
 
     // Compute the Lagrange multiplier lambda
     Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b);
@@ -128,3 +129,8 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData)
     }
 }
 
+// Solve the position constraint
+void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
+
+}
+

src/constraint/BallAndSocketJoint.h

@@ -43,11 +43,14 @@ struct BallAndSocketJointInfo : public ConstraintInfo {
 
         // -------------------- Attributes -------------------- //
 
-        /// Anchor point (in world space coordinates)
+        /// Anchor point (in world-space coordinates)
         Vector3 anchorPointWorldSpace;
 
         /// Constructor
-        BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {}
+        BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                               const Vector3& initAnchorPointWorldSpace)
+                              : ConstraintInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
+                                anchorPointWorldSpace(initAnchorPointWorldSpace){}
 };
 
 // Class BallAndSocketJoint
@@ -101,8 +104,11 @@ class BallAndSocketJoint : public Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
 
-        /// Solve the constraint
+        /// Solve the velocity constraint
-        virtual void solve(const ConstraintSolverData& constraintSolverData);
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the position constraint
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
 };
 
 // Return the number of bytes used by the joint

src/constraint/Constraint.cpp

@@ -31,7 +31,8 @@ using namespace reactphysics3d;
 // Constructor
 Constraint::Constraint(const ConstraintInfo& constraintInfo)
            :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
-            mType(constraintInfo.type) {
+            mType(constraintInfo.type),
+            mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique) {
 
     assert(mBody1 != NULL);
     assert(mBody2 != NULL);

src/constraint/Constraint.h

@@ -27,6 +27,7 @@
 #define REACTPHYSICS3D_CONSTRAINT_H
 
 // Libraries
+#include "../configuration.h"
 #include "../body/RigidBody.h"
 #include "../mathematics/mathematics.h"
 
@@ -34,7 +35,7 @@
 namespace reactphysics3d {
     
 // Enumeration for the type of a constraint
-enum ConstraintType {CONTACT, BALLSOCKETJOINT};
+enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT};
 
 // Class declarations
 struct ConstraintSolverData;
@@ -58,13 +59,19 @@ struct ConstraintInfo {
         /// Type of the constraint
         ConstraintType type;
 
+        /// Position correction technique used for the constraint (used for joints).
+        /// By default, the BAUMGARTE technique is used
+        JointsPositionCorrectionTechnique positionCorrectionTechnique;
+
         /// Constructor
         ConstraintInfo(ConstraintType constraintType)
-                      : body1(NULL), body2(NULL), type(constraintType) {}
+                      : body1(NULL), body2(NULL), type(constraintType),
+                        positionCorrectionTechnique(BAUMGARTE_JOINTS) {}
 
         /// Constructor
         ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType)
-                      : body1(rigidBody1), body2(rigidBody2), type(constraintType) {
+                      : body1(rigidBody1), body2(rigidBody2), type(constraintType),
+                        positionCorrectionTechnique(BAUMGARTE_JOINTS) {
         }
 
         /// Destructor
@@ -103,6 +110,9 @@ class Constraint {
         /// Body 2 index in the velocity array to solve the constraint
         uint mIndexBody2;
 
+        /// Position correction technique used for the constraint (used for joints)
+        JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -139,8 +149,11 @@ class Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
 
-        /// Solve the constraint
+        /// Solve the velocity constraint
-        virtual void solve(const ConstraintSolverData& constraintSolverData) = 0;
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
+
+        /// Solve the position constraint
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
 };
 
 // Return the reference to the body 1

src/constraint/ContactPoint.cpp

@@ -56,7 +56,12 @@ void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverD
 
 }
 
-// Solve the constraint
+// Solve the velocity constraint
-void ContactPoint::solve(const ConstraintSolverData& constraintSolverData) {
+void ContactPoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+
+}
+
+// Solve the position constraint
+void ContactPoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
 
 }

src/constraint/ContactPoint.h

@@ -227,8 +227,11 @@ class ContactPoint : public Constraint {
         /// Initialize before solving the constraint
         virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
 
-        /// Solve the constraint
+        /// Solve the velocity constraint
-        virtual void solve(const ConstraintSolverData& constraintSolverData);
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the position constraint
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
 
         #ifdef VISUAL_DEBUG
             /// Draw the contact (for debugging)

src/constraint/SliderJoint.cpp

@@ -0,0 +1,105 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "SliderJoint.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo) {
+
+    // Compute the local-space anchor point for each body
+    mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+}
+
+// Destructor
+SliderJoint::~SliderJoint() {
+
+}
+
+// Initialize before solving the constraint
+void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
+
+    // Initialize the bodies index in the velocity array
+    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+
+    // Get the bodies positions and orientations
+    const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
+    const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
+
+    // Get the inertia tensor of bodies
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mU1World = orientationBody1 * mLocalAnchorPointBody1;
+    mU2World = orientationBody2 * mLocalAnchorPointBody2;
+
+    // Compute the two orthogonal vectors to vector mU1World in world-space
+    mN1 = mU1World.getOneUnitOrthogonalVector();
+    mN2 = mU1World.cross(mN1);
+
+    // Compute the cross product 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)
+    const decimal n1Dotn1 = mN1.lengthSquare();
+    const decimal n2Dotn2 = mN2.lengthSquare();
+    const decimal sumInverseMass = mBody1->getMassInverse() + mBody2->getMassInverse();
+
+}
+
+// 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];
+    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 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
+    Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute J*v
+}
+
+// Solve the position constraint
+void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
+
+}

src/constraint/SliderJoint.h

@@ -0,0 +1,131 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SLIDER_JOINT_H
+#define REACTPHYSICS3D_SLIDER_JOINT_H
+
+// Libraries
+#include "../mathematics/mathematics.h"
+#include "../engine/ConstraintSolver.h"
+
+namespace reactphysics3d {
+
+// Structure SliderJointInfo
+/**
+ * This structure is used to gather the information needed to create a slider
+ * joint. This structure will be used to create the actual slider joint.
+ */
+struct SliderJointInfo : public ConstraintInfo {
+
+    public :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point (in world-space coordinates)
+        Vector3 anchorPointWorldSpace;
+
+        /// Slider axis (in world-space coordinates)
+        Vector3 axisWorldSpace;
+
+        /// Constructor
+        SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                        const Vector3& initAnchorPointWorldSpace,
+                        const Vector3& initAxisWorldSpace)
+                       : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
+                         anchorPointWorldSpace(initAnchorPointWorldSpace),
+                         axisWorldSpace(initAxisWorldSpace) {}
+};
+
+// Class SliderJoint
+/**
+ * This class represents a slider joint.
+ */
+class SliderJoint : public Constraint {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point of body 1 (in local space coordinates)
+        Vector3 mLocalAnchorPointBody1;
+
+        /// Anchor point of body 2 (in local space coordinates)
+        Vector3 mLocalAnchorPointBody2;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mU1World;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mU2World;
+
+        /// First vector orthogonal to vector mU1World in world-space
+        Vector3 mN1;
+
+        /// Second vector orthogonal to vector mU1World and mN1 in world-space
+        Vector3 mN2;
+
+        /// Cross product of mU1World and mN1
+        Vector3 mU1WorldCrossN1;
+
+        /// Cross product of mU1World and mN2
+        Vector3 mU1WorldCrossN2;
+
+        /// Cross product of mU2World and mN1
+        Vector3 mU2WorldCrossN1;
+
+        /// Cross product of mU2World and mN2
+        Vector3 mU2WorldCrossN2;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        SliderJoint(const SliderJointInfo& jointInfo);
+
+        /// Destructor
+        virtual ~SliderJoint();
+
+        /// Return the number of bytes used by the joint
+        virtual size_t getSizeInBytes() const;
+
+        /// Initialize before solving the constraint
+        virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the velocity constraint
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the position constraint
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
+};
+
+// Return the number of bytes used by the joint
+inline size_t SliderJoint::getSizeInBytes() const {
+    return sizeof(SliderJoint);
+}
+
+}
+
+#endif

src/engine/ConstraintSolver.cpp

@@ -37,7 +37,9 @@ ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
                  : mJoints(joints), mLinearVelocities(linearVelocities),
                    mAngularVelocities(angularVelocities),
                    mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(false), mConstraintSolverData(linearVelocities,
+                   mIsWarmStartingActive(false),
+                   mIsNonLinearGaussSeidelPositionCorrectionActive(false),
+                   mConstraintSolverData(linearVelocities,
                    angularVelocities, mapBodyToVelocityIndex){
 
 }
@@ -78,10 +80,10 @@ void ConstraintSolver::initialize(decimal dt) {
     }
 }
 
-// Solve the constraints
+// Solve the velocity constraints
-void ConstraintSolver::solve() {
+void ConstraintSolver::solveVelocityConstraints() {
 
-    PROFILE("ConstraintSolver::solve()");
+    PROFILE("ConstraintSolver::solveVelocityConstraints()");
 
     // For each joint
     std::set<Constraint*>::iterator it;
@@ -90,6 +92,22 @@ void ConstraintSolver::solve() {
         Constraint* joint = (*it);
 
         // Solve the constraint
-        joint->solve(mConstraintSolverData);
+        joint->solveVelocityConstraint(mConstraintSolverData);
+    }
+}
+
+// Solve the position constraints
+void ConstraintSolver::solvePositionConstraints() {
+
+    PROFILE("ConstraintSolver::solvePositionConstraints()");
+
+    // For each joint
+    std::set<Constraint*>::iterator it;
+    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+
+        Constraint* joint = (*it);
+
+        // Solve the constraint
+        joint->solveVelocityConstraint(mConstraintSolverData);
     }
 }

src/engine/ConstraintSolver.h

@@ -171,6 +171,9 @@ class ConstraintSolver {
         /// True if the warm starting of the solver is active
         bool mIsWarmStartingActive;
 
+        /// True if the Non-Linear-Gauss-Seidel position correction technique is enabled
+        bool mIsNonLinearGaussSeidelPositionCorrectionActive;
+
         /// Constraint solver data used to initialize and solve the constraints
         ConstraintSolverData mConstraintSolverData;
 
@@ -191,9 +194,28 @@ class ConstraintSolver {
         void initialize(decimal dt);
 
         /// Solve the constraints
-        void solve();
+        void solveVelocityConstraints();
+
+        /// Solve the position constraints
+        void solvePositionConstraints();
+
+        /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
+        bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
+
+        /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
+        void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
 };
 
+// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
+inline bool ConstraintSolver::getIsNonLinearGaussSeidelPositionCorrectionActive() const {
+    return mIsNonLinearGaussSeidelPositionCorrectionActive;
+}
+
+// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
+inline void ConstraintSolver::setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive) {
+    mIsNonLinearGaussSeidelPositionCorrectionActive = isActive;
+}
+
 }
 
 #endif

src/engine/DynamicsWorld.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "DynamicsWorld.h"
 #include "constraint/BallAndSocketJoint.h"
+#include "constraint/SliderJoint.h"
 
 // Namespaces
 using namespace reactphysics3d;
@@ -38,7 +39,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
                                mMapBodyToConstrainedVelocityIndex),
                 mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
                                   mMapBodyToConstrainedVelocityIndex),
-                mNbSolverIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
+                mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
+                mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
                 mIsDeactivationActive(DEACTIVATION_ENABLED) {
 
 }
@@ -248,6 +250,8 @@ void DynamicsWorld::solveContactsAndConstraints() {
     bool isContactsToSolve = !mContactManifolds.empty();
     if (!isConstraintsToSolve && !isContactsToSolve) return;
 
+    // ---------- Solve velocity constraints for joints and contacts ---------- //
+
     // If there are contacts
     if (isContactsToSolve) {
 
@@ -265,11 +269,11 @@ void DynamicsWorld::solveContactsAndConstraints() {
         mConstraintSolver.initialize(dt);
     }
 
-    // For each iteration of the solver
+    // For each iteration of the velocity solver
-    for (uint i=0; i<mNbSolverIterations; i++) {
+    for (uint i=0; i<mNbVelocitySolverIterations; i++) {
 
-        // Solve the constraints
+        // Solve the velocity constraints
-        if (isConstraintsToSolve) mConstraintSolver.solve();
+        if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
 
         // Solve the contacts
         if (isContactsToSolve) mContactSolver.solve();
@@ -277,6 +281,21 @@ void DynamicsWorld::solveContactsAndConstraints() {
 
     // Cache the lambda values in order to use them in the next step
     if (isContactsToSolve) mContactSolver.storeImpulses();
+
+    // ---------- Solve positions constraints (error correction) for joints only ---------- //
+
+    // TODO : Integrate the bodies positions here
+
+    // If the Non-Linear-Gauss-Seidel position correction is active
+    if (mConstraintSolver.getIsNonLinearGaussSeidelPositionCorrectionActive()) {
+
+        // For each iteration of the position (error correction) solver
+        for (uint i=0; i<mNbPositionSolverIterations; i++) {
+
+            // Solve the position constraints
+            if (isConstraintsToSolve) mConstraintSolver.solvePositionConstraints();
+        }
+    }
 }
 
 // Cleanup the constrained velocities array at each step
@@ -385,6 +404,15 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
             break;
         }
 
+        // Slider joint
+        case SLIDERJOINT:
+        {
+            void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint));
+            const SliderJointInfo& info = dynamic_cast<const SliderJointInfo&>(jointInfo);
+            newJoint = new (allocatedMemory) SliderJoint(info);
+            break;
+        }
+
         default:
         {
             assert(false);

src/engine/DynamicsWorld.h

@@ -59,8 +59,11 @@ class DynamicsWorld : public CollisionWorld {
         /// Constraint solver
         ConstraintSolver mConstraintSolver;
 
-        /// Number of solver iterations for the Sequential Impulses technique
+        /// Number of iterations for the velocity solver of the Sequential Impulses technique
-        uint mNbSolverIterations;
+        uint mNbVelocitySolverIterations;
+
+        /// Number of iterations for the position solver of the Sequential Impulses technique
+        uint mNbPositionSolverIterations;
 
         /// True if the deactivation (sleeping) of inactive bodies is enabled
         bool mIsDeactivationActive;
@@ -155,19 +158,22 @@ public :
         /// Update the physics simulation
         void update();
 
-        /// Set the number of iterations of the constraint solver
+        /// Set the number of iterations for the velocity constraint solver
-        void setNbIterationsSolver(uint nbIterations);
+        void setNbIterationsVelocitySolver(uint nbIterations);
 
-        /// Activate or Deactivate the split impulses for contacts
+        /// Set the number of iterations for the position constraint solver
-        void setIsSplitImpulseActive(bool isActive);
+        void setNbIterationsPositionSolver(uint nbIterations);
+
+        /// Set the position correction technique used for contacts
+        void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
+
+        /// Set the position correction technique used for joints
+        void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
 
         /// Activate or deactivate the solving of friction constraints at the center of
         /// the contact manifold instead of solving them at each contact point
         void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
 
-        /// Set the isErrorCorrectionActive value
-        void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
-
         /// Create a rigid body into the physics world.
         RigidBody* createRigidBody(const Transform& transform, decimal mass,
                                    const Matrix3x3& inertiaTensorLocal,
@@ -213,14 +219,36 @@ inline void DynamicsWorld::stop() {
     mTimer.stop();
 }                
 
-// Set the number of iterations of the constraint solver
+// Set the number of iterations for the velocity constraint solver
-inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
+inline void DynamicsWorld::setNbIterationsVelocitySolver(uint nbIterations) {
-    mNbSolverIterations = nbIterations;
+    mNbVelocitySolverIterations = nbIterations;
 }
 
-// Activate or Deactivate the split impulses for contacts
+// Set the number of iterations for the position constraint solver
-inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) {
+inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
-    mContactSolver.setIsSplitImpulseActive(isActive);
+    mNbPositionSolverIterations = nbIterations;
+}
+
+// Set the position correction technique used for contacts
+inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
+                              ContactsPositionCorrectionTechnique technique) {
+    if (technique == BAUMGARTE_CONTACTS) {
+        mContactSolver.setIsSplitImpulseActive(false);
+    }
+    else {
+        mContactSolver.setIsSplitImpulseActive(true);
+    }
+}
+
+// Set the position correction technique used for joints
+inline void DynamicsWorld::setJointsPositionCorrectionTechnique(
+                              JointsPositionCorrectionTechnique technique) {
+    if (technique == BAUMGARTE_JOINTS) {
+        mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
+    }
+    else {
+        mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
+    }
 }
 
 // Activate or deactivate the solving of friction constraints at the center of