diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp
index f897de9a..3df8c140 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.cpp
+++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp
@@ -19,58 +19,124 @@
 
 // Libraries
 #include "ConstraintSolver.h"
+#include "LCPProjectedGaussSeidel.h"
+
+using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver() {
-
+ConstraintSolver::ConstraintSolver()
+                 :bodies(0), nbBodies(0), bodyMapping(0) {
+    // Creation of the LCP Solver
+    lcpSolver = new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS);
 }
 
 // Destructor
-ConstraintSolver~ConstraintSolver() {
+ConstraintSolver::~ConstraintSolver() {
 
 }
 
 // Allocate all the matrices needed to solve the LCP problem
-void ConstraintSolver::allocate(std::vector<Constraint*>& constraints, double dt) {
-    std::vector<Constraint*> activeConstraints;
+void ConstraintSolver::allocate(std::vector<Constraint*>& constraints, std::vector<Body*>& bodies) {
     unsigned int sizeJacobian = 0;
-    unsigned int nbAuxiliaryVars = 0;
+    this->bodies = bodies;
+    this->nbBodies = bodies.size();
 
     // For each constraint
     for (unsigned int i=0; i<constraints.size(); ++i) {
+        // Evaluate the constraint
+        constraints.at(i)->evaluate(dt);
+
         // If the constraint is active
         if (constraints.at(i)->isActive()) {
             activeConstraints.push_back(constraints.at(i));
 
-            // Evaluate the constraint
-            constraints.at(i)->evaluate(dt);
-
-            // Set the current jacobian index to the constraint
-            constraints.at(i)->setJacobianIndex(sizeJacobian);
-
-            // Update the size of the jacobian matrix
-            sizeJacobian += constraints.at(i)->getNbJacobianRows();
+            /Description/ Update the size of the jacobian matrix
+            sizeJacobian += (1 + constraints.at(i)->getNbAuxConstraints());
         }
     }
 
-    // For each active constraint
-    for (unsigned int i=0; i<activeConstraints.size(); ++i) {
-        // Set the auxiliary index
-        activeConstraints.at(i)->setAuxiliaryIndex(sizeJacobian + nbAuxiliaryVars);
+    // Allocate all the vectors and matrices
+    J_sp = Matrix(sizeJacobian, 12);
 
-        // Update the number of auxiliary variables
-        nbAuxiliaryVars += activeConstraints.at(i)->getNbAuxiliaryVars();
+    bodyMapping = new unsigned int[nbBodies];
+    for (unsigned int i=0; i<nbBodies; i++) {
+        bodyMapping[i] = new unsigned int[2];
     }
 
-    // TODO : Continue to implement this ...
+    errorVector = Vector(sizeJacobian);
+    B_sp = Matrix(6*nbBodies, 2;
+    b = Vector(totalSize);
+    lambda = Vector(totalSize);
+    lowLimits = Vector(totalSize);
+    highLimits = Vector(totalSize);
+    Minv_sp = Matrix(3*nbBodies, 6*nbBodies);
+    V = Vector(6*nbBodies);
+    Fc = Vector(6*nbBodies);
 }
 
 // Fill in all the matrices needed to solve the LCP problem
-void ConstraintSolver::fillInMatrices(std::vector<Constraint*> constraints) {
+// Notice that all the active constraints should have been evaluated first
+void ConstraintSolver::fillInMatrices() {
+    int i,j;
 
+    // For each active constraint
+    for (unsigned int c=0; c<activeConstraints.size(); ++c) {
+        i = activeConstraints.at(c)->getJacobianIndex();
+        //j = i + activeConstraint.at(c)->getNbJacobianRows();
+        J.fillInSubMatrix(i, 0, activeConstraints.at(c)->getBody1LinearJacobian());
+        J.fillInSubMatrix(i, 3, activeConstraints.at(c)->getBody2LinearJacobian());
+        J.fillInSubMatrix(i, 6, activeConstraints.at(c)->getBody1AngularJacobian());
+        J.fillInSubMatrix(i, 9, activeConstraints.at(c)->getBody2AngularJacobian());
+        errorVector.fillInSubVector(i, activeConstraints.at(c)->getErrorVector());
+    }
+
+    // For each active constraint
+    for (unsigned int c=0; c<activeConstraints.size(); ++c) {
+        i = activeConstraints.at(c)->getNbAuxiliaryVars();
+        // TODO : add activeConstraints.at(c)->getAuxiliaryRowsAndCols(..., ...)
+        b.fillInSubVector(i, activeConstraints.at(c)->getRightHandSideVector());
+    }
+
+    // For each current body of the simulation
+    for (unsigned int b=0; b<nbBodies; ++b) {
+        i = 6*b;
+        Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
+        Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse());
+        u.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getLinearVelocity());
+        u.fillInSubVector(i+3, bodies.at(b)->getCurrentBodyState().getAngularVelocity());
+        fExt.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getExternalForce());
+        Vector3D externalTorque = bodies.at(b)->getCurrentBodyState().getExternalTorque();
+        Matrix3x3 inertia = bodies.at(b)->getInertiaTensor();
+        Vector3D angularVelocity = bodies.at(b)->getCurrentBodyState().getAngularVelocity();
+        fExt.fillInSubVector(i+3, externalTorque - (angularVelocity.crossProduct(inertia*angularVelocity)));
+    }
+}
+
+// Free the memory that was allocated in the allocate() method
+void ConstraintSolver::freeMemory() {
+    // Free the bodyMaping array
+    for (unsigned int i=0; i<nbBodies; i++) {
+        delete[] bodyMapping[i];
+    }
+    delete[] bodyMapping;
 }
 
 // Solve the current LCP problem
-void ConstraintSolver::solve(std::vector<Constraint*> constraints, double dt) {
+void ConstraintSolver::solve(std::vector<Constraint*>& constraints, std::vector<Body*>* bodies, double dt) {
+    // Update the current set of bodies in the physics world
+    this->bodies = bodies;
 
+    // Delete all the current actice constraints
+    activeConstraints.clear();
+
+    // Allocate memory for the matrices
+    allocate(constraints, dt);
+
+    // Fill-in all the matrices needed to solve the LCP problem
+    fillInMatrices();
+
+    // Solve the LCP problem (computation of lambda)
+    lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
+
+    // TODO : Implement this method ...
 }
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h
index 9c5e9ad4..078c4bb9 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.h
+++ b/sources/reactphysics3d/engine/ConstraintSolver.h
@@ -26,6 +26,9 @@
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Constants
+const unsigned int MAX_LCP_ITERATIONS = 10;     // Maximum number of iterations when solving a LCP problem
+
  /*  -------------------------------------------------------------------
     Class ConstrainSolver :
         This class represents the constraint solver. The goal is to
@@ -34,13 +37,32 @@ namespace reactphysics3d {
 */
 class ConstraintSolver {
     protected:
-        void allocate(std::vector<Constraint*>& constraints, double dt);    // Allocate all the matrices needed to solve the LCP problem
-        void fillInMatrices(std::vector<Constraint*> constraints);         // Fill in all the matrices needed to solve the LCP problem
+        LCPSolver* lcpSolver;                                               // LCP Solver
+        std::vector<Constraint*> activeConstraints;                         // Current active constraints in the physics world
+        std::vector<Body*> bodies;                                          // Set of bodies in the physics world
+        unsigned int nbBodies;                                              // Number of bodies in the physics world
+        unsigned int** bodyMapping                                          // 2-dimensional array that contains the mapping of body index
+                                                                            // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
+                                                                            // the integer index of the body that correspond to the 1x6 J_ij matrix in the
+                                                                            // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
+        Matrix J_sp;                                                        // Sparse representation of the jacobian matrix of all constraints
+        Matrix B_sp;                                                        // Useful matrix in sparse representation
+        Vector b;                                                           // Vector "b" of the LCP problem
+        Vector lambda;                                                      // Lambda vector of  the LCP problem
+        Vector errorVector;                                                 // Error vector of all constraints
+        Vector lowLimits;                                                   // Vector that contains the low limits of the LCP problem
+        Vector highLimits;                                                  // Vector that contains the high limits of the LCP problem
+        Matrix Minv_sp;                                                     // Sparse representation of the Matrix that contains information about mass and inertia of each body
+        Vector V;                                                           // Vector that contains internal linear and angular velocities of each body
+        Vector Fc;                                                          // Vector that contains internal forces and torques of each body due to constraints
+        void allocate(std::vector<Constraint*>& constraints);               // Allocate all the matrices needed to solve the LCP problem
+        void fillInMatrices();                                              // Fill in all the matrices needed to solve the LCP problem
+        void freeMemory();                                                  // Free the memory that was allocated in the allocate() method
 
     public:
-        ConstraintSolver();                                                 // Constructor
-        virtual ~ConstraintSolver();                                        // Destructor
-        void solve(std::vector<Constraint*>& constraints, double dt);       // Solve the current LCP problem
+        ConstraintSolver();                                                                         // Constructor
+        virtual ~ConstraintSolver();                                                                // Destructor
+        void solve(std::vector<Constraint*>& constraints, std::vector<Body*>* bodies, double dt);   // Solve the current LCP problem
 };
 
 } // End of ReactPhysics3D namespace