diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 87e0f3e4..5190d1f4 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -34,7 +34,8 @@ using namespace std;
 
 // Constructor
 ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
-    :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), Vconstraint(0), Wconstraint(0), V1(0), W1(0) {
+    :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0),
+      mLinearVelocities(0), mAngularVelocities(0) {
 
 }
 
@@ -45,7 +46,7 @@ ConstraintSolver::~ConstraintSolver() {
 
  // Initialize the constraint solver before each solving
 void ConstraintSolver::initialize() {
-    
+
     nbConstraints = 0;
 
     // TOOD : Use better allocation here
@@ -111,10 +112,8 @@ void ConstraintSolver::initialize() {
     // Compute the number of bodies that are part of some active constraint
     nbBodies = mConstraintBodies.size();
 
-    Vconstraint = new Vector3[nbBodies];
-    Wconstraint = new Vector3[nbBodies];
-    V1 = new Vector3[nbBodies];
-    W1 = new Vector3[nbBodies];
+    mLinearVelocities = new Vector3[nbBodies];
+    mAngularVelocities = new Vector3[nbBodies];
 
     assert(mMapBodyToIndex.size() == nbBodies);
 }
@@ -135,12 +134,9 @@ void ConstraintSolver::initializeBodies() {
 
         // Compute the vector V1 with initial velocities values
         int bodyIndexArray = 6 * bodyNumber;
-        V1[bodyNumber] = rigidBody->getLinearVelocity();
-        W1[bodyNumber] = rigidBody->getAngularVelocity();
 
-        // Compute the vector Vconstraint with final constraint velocities
-        Vconstraint[bodyNumber] = Vector3(0, 0, 0);
-        Wconstraint[bodyNumber] = Vector3(0, 0, 0);
+        mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
+        mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
 
         // Initialize the mass and inertia tensor matrices
         Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
@@ -225,8 +221,6 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
 
             // Fill in the error vector
             realContact->computeErrorPenetration(contact.errorPenetration);
-            contact.errorFriction1 = 0.0;
-            contact.errorFriction2 = 0.0;
 
             // Get the cached lambda values of the constraint
             contact.penetrationImpulse = realContact->getCachedLambda(0);
@@ -239,27 +233,15 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
             contact.errorPenetration = 0.0;
             decimal slop = 0.005;
             if (realContact->getPenetrationDepth() > slop) {
-                 contact.errorPenetration += 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0);
+                 contact.errorPenetration -= 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0);
             }
-            contact.errorFriction1 = 0.0;
-            contact.errorFriction2 = 0.0;
 
             // ---------- Penetration ---------- //
 
             // b = errorValues * oneOverDT;
-            contact.b_Penetration = contact.errorPenetration * oneOverDT;
-
-             // ---------- Friction 1 ---------- //
-
-            contact.b_Friction1 = contact.errorFriction1 * oneOverDT;
-
-             // ---------- Friction 2 ---------- //
-
-            contact.b_Friction2 = contact.errorFriction2 * oneOverDT;
+            contact.b_Penetration = contact.errorPenetration;
         }
     }
-
-
 }
 
 // Compute the matrix B_sp
@@ -373,67 +355,9 @@ void ConstraintSolver::computeMatrixB_sp() {
     }
 }
 
-// Compute the vector V_constraint (which corresponds to the constraint part of
-// the final V2 vector) according to the formula
-// V_constraint = dt * (M^-1 * J^T * lambda)
-// Note that we use the vector V to store both V1 and V_constraint.
-// Note that M^-1 * J^T = B.
-// This method is called after that the LCP solver has computed lambda
-void ConstraintSolver::computeVectorVconstraint(decimal dt) {
-    uint indexBody1Array, indexBody2Array;
-	uint j;
-	
-    // Compute dt * (M^-1 * J^T * lambda
-    for (uint c=0; c<mNbContactConstraints; c++) {
-
-        ContactConstraint& constraint = mContactConstraints[c];
-
-        for (uint i=0; i<constraint.nbContacts; i++) {
-
-            ContactPointConstraint& contact = constraint.contacts[i];
-
-            // ---------- Penetration ---------- //
-
-            indexBody1Array = constraint.indexBody1;
-            indexBody2Array = constraint.indexBody2;
-            for (j=0; j<3; j++) {
-                Vconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt;
-                Wconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse * dt;
-
-                Vconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt;
-                Wconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse * dt;
-            }
-
-            // ---------- Friction 1 ---------- //
-
-            for (j=0; j<3; j++) {
-                Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt;
-                Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse * dt;
-
-                Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt;
-                Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse * dt;
-            }
-
-            // ---------- Friction 2 ---------- //
-
-            for (j=0; j<3; j++) {
-                Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt;
-                Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse * dt;
-
-                Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt;
-                Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse * dt;
-            }
-        }
-    }
-}
-
 // Solve a LCP problem using the Projected-Gauss-Seidel algorithm
 // This method outputs the result in the lambda vector
-void ConstraintSolver::solveLCP(decimal dt) {
-
-   // for (uint i=0; i<nbConstraints; i++) {
-   //         lambda[i] = lambdaInit[i];
-   // }
+void ConstraintSolver::solveLCP() {
 
     uint indexBody1Array, indexBody2Array;
     decimal deltaLambda;
@@ -441,7 +365,7 @@ void ConstraintSolver::solveLCP(decimal dt) {
     uint iter;
 
     // Compute the vector a
-    computeVectorA(dt);
+    warmStart();
 
     // For each iteration
     for(iter=0; iter<mNbIterations; iter++) {
@@ -460,59 +384,60 @@ void ConstraintSolver::solveLCP(decimal dt) {
 
                 // --------- Penetration --------- //
 
-                deltaLambda = contact.b_Penetration;
+                deltaLambda = 0.0;
                 for (uint j=0; j<3; j++) {
-                    deltaLambda -= (contact.J_spBody1Penetration[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Penetration[j] * aLinear[indexBody2Array][j]);
-                    deltaLambda -= (contact.J_spBody1Penetration[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * aAngular[indexBody2Array][j]);
+                    deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]);
+                    deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]);
                 }
+                deltaLambda -= contact.b_Penetration;
                 deltaLambda /= contact.inversePenetrationMass;
                 lambdaTemp = contact.penetrationImpulse;
                 contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
                 deltaLambda = contact.penetrationImpulse - lambdaTemp;
                 for (uint j=0; j<3; j++) {
-                    aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
-                    aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;
+                    mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
+                    mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;
 
-                    aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
-                    aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
+                    mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
+                    mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
                 }
 
                 // --------- Friction 1 --------- //
 
-                deltaLambda = contact.b_Friction1;
+                deltaLambda = 0.0;
                 for (uint j=0; j<3; j++) {
-                    deltaLambda -= (contact.J_spBody1Friction1[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction1[j] * aLinear[indexBody2Array][j]);
-                    deltaLambda -= (contact.J_spBody1Friction1[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * aAngular[indexBody2Array][j]);
+                    deltaLambda -= (contact.J_spBody1Friction1[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j] * mLinearVelocities[indexBody2Array][j]);
+                    deltaLambda -= (contact.J_spBody1Friction1[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * mAngularVelocities[indexBody2Array][j]);
                 }
                 deltaLambda /= contact.inverseFriction1Mass;
                 lambdaTemp = contact.friction1Impulse;
                 contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1));
                 deltaLambda = contact.friction1Impulse - lambdaTemp;
                 for (uint j=0; j<3; j++) {
-                    aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
-                    aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;
+                    mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
+                    mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;
 
-                    aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
-                    aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
+                    mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
+                    mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
                 }
 
                 // --------- Friction 2 --------- //
 
-                deltaLambda = contact.b_Friction2;
+                deltaLambda = 0.0;
                 for (uint j=0; j<3; j++) {
-                    deltaLambda -= (contact.J_spBody1Friction2[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction2[j] * aLinear[indexBody2Array][j]);
-                    deltaLambda -= (contact.J_spBody1Friction2[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * aAngular[indexBody2Array][j]);
+                    deltaLambda -= (contact.J_spBody1Friction2[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j] * mLinearVelocities[indexBody2Array][j]);
+                    deltaLambda -= (contact.J_spBody1Friction2[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * mAngularVelocities[indexBody2Array][j]);
                 }
                 deltaLambda /= contact.inverseFriction2Mass;
                 lambdaTemp = contact.friction2Impulse;
                 contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2));
                 deltaLambda = contact.friction2Impulse - lambdaTemp;
                 for (uint j=0; j<3; j++) {
-                    aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
-                    aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;
+                    mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
+                    mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;
 
-                    aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda;
-                    aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda;
+                    mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda;
+                    mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda;
                 }
             }
         }
@@ -521,19 +446,9 @@ void ConstraintSolver::solveLCP(decimal dt) {
 
 // Compute the vector a used in the solve() method
 // Note that a = B * lambda
-void ConstraintSolver::computeVectorA(decimal dt) {
+void ConstraintSolver::warmStart() {
     uint i;
     uint indexBody1Array, indexBody2Array;
-    decimal oneOverDt = 1.0 / dt;
-    
-    // Init the vector a with zero values
-    for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
-        RigidBody* rigidBody = *it;
-        uint bodyNumber = mMapBodyToIndex[rigidBody];
-
-        aLinear[bodyNumber] = oneOverDt * V1[bodyNumber] + rigidBody->getMassInverse() * rigidBody->getExternalForce();
-        aAngular[bodyNumber] = oneOverDt * W1[bodyNumber] + rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
-    }
 
     // For each constraint
     for (uint c=0; c<mNbContactConstraints; c++) {
@@ -550,31 +465,31 @@ void ConstraintSolver::computeVectorA(decimal dt) {
             // --------- Penetration --------- //
 
             for (uint j=0; j<3; j++) {
-                aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
-                aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
+                mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
+                mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
 
-                aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
-                aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse;
+                mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
+                mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse;
             }
 
             // --------- Friction 1 --------- //
 
             for (uint j=0; j<3; j++) {
-                aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
-                aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
+                mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
+                mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
 
-                aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
-                aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse;
+                mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
+                mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse;
             }
 
             // --------- Friction 2 --------- //
 
             for (uint j=0; j<3; j++) {
-                aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
-                aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
+                mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
+                mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
 
-                aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
-                aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
+                mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
+                mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
             }
         }
     }
@@ -599,3 +514,26 @@ void ConstraintSolver::cacheLambda() {
         }
     }
 }
+
+// Solve the current LCP problem
+void ConstraintSolver::solve(decimal dt) {
+
+    mTimeStep = dt;
+
+    // Initialize the solver
+    initialize();
+
+    initializeBodies();
+
+    // Fill-in all the matrices needed to solve the LCP problem
+    initializeContactConstraints(dt);
+
+    // Compute the matrix B
+    computeMatrixB_sp();
+
+    // Solve the LCP problem (computation of lambda)
+    solveLCP();
+
+    // Cache the lambda values in order to use them in the next step
+    cacheLambda();
+}
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index c5ae3a80..3ac82eb6 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -77,12 +77,8 @@ struct ContactPointConstraint {
     decimal lowerBoundFriction2;
     decimal upperBoundFriction2;
     decimal errorPenetration;
-    decimal errorFriction1;
-    decimal errorFriction2;
     Contact* contact;                       // TODO : REMOVE THIS
     decimal b_Penetration;
-    decimal b_Friction1;
-    decimal b_Friction2;
     decimal B_spBody1Penetration[6];
     decimal B_spBody2Penetration[6];
     decimal B_spBody1Friction1[6];
@@ -160,8 +156,6 @@ class ConstraintSolver {
         decimal b[NB_MAX_CONSTRAINTS];                  // Vector "b" of the LCP problem
         decimal bError[NB_MAX_CONSTRAINTS];             // Vector "b" of the LCP problem for error correction projection
         decimal d[NB_MAX_CONSTRAINTS];                  // Vector "d"
-        Vector3 aLinear[NB_MAX_BODIES];                     // Vector "a"
-        Vector3 aAngular[NB_MAX_BODIES];
         decimal aError[6*NB_MAX_BODIES];                // Vector "a" for error correction projection
         decimal penetrationDepths[NB_MAX_CONSTRAINTS];  // Array of penetration depths for error correction projection
         decimal lambda[NB_MAX_CONSTRAINTS];             // Lambda vector of the LCP problem
@@ -175,11 +169,9 @@ class ConstraintSolver {
         Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES];       // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
         decimal Minv_sp_mass_diag[NB_MAX_BODIES];       // Array that contains for each body the inverse of its mass
                                                         // This is an array of size nbBodies that contains in each cell a 6x6 matrix
-        Vector3* V1;                    // Array that contains for each body the 6x1 vector that contains linear and angular velocities
-        Vector3* W1;
-        Vector3* Vconstraint;                           // Same kind of vector as V1 but contains the final constraint velocities
-        Vector3* Wconstraint;
-        decimal VconstraintError[6*NB_MAX_BODIES];      // Same kind of vector as V1 but contains the final constraint velocities
+        Vector3* mLinearVelocities;                     // Array of constrained linear velocities
+        Vector3* mAngularVelocities;                    // Array of constrained angular velocities
+        decimal mTimeStep;                              // Current time step
 
         // Contact constraints
         ContactConstraint* mContactConstraints;
@@ -198,10 +190,9 @@ class ConstraintSolver {
         void initializeBodies();                        // Initialize bodies velocities
         void initializeContactConstraints(decimal dt);                // Fill in all the matrices needed to solve the LCP problem
         void computeMatrixB_sp();                       // Compute the matrix B_sp
-        void computeVectorVconstraint(decimal dt);      // Compute the vector V2
         void cacheLambda();                             // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
-        void computeVectorA(decimal dt);                          // Compute the vector a used in the solve() method
-        void solveLCP(decimal dt);                                // Solve a LCP problem using Projected-Gauss-Seidel algorithm
+        void warmStart();                          // Compute the vector a used in the solve() method
+        void solveLCP();                                // Solve a LCP problem using Projected-Gauss-Seidel algorithm
     
    public:
         ConstraintSolver(DynamicsWorld* world);                         // Constructor
@@ -223,14 +214,14 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
 inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
     assert(isConstrainedBody(body));
     uint indexBodyArray = mMapBodyToIndex[body];
-    return Vconstraint[indexBodyArray];
+    return mLinearVelocities[indexBodyArray];
 }
 
 // Return the constrained angular velocity of a body after solving the LCP problem
 inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
     assert(isConstrainedBody(body));
     uint indexBodyArray = mMapBodyToIndex[body];
-    return Wconstraint[indexBodyArray];
+    return mAngularVelocities[indexBodyArray];
 }
 
 // Cleanup of the constraint solver
@@ -243,51 +234,19 @@ inline void ConstraintSolver::cleanup() {
         delete[] mContactConstraints;
         mContactConstraints = 0;
     }
-    if (Vconstraint != 0) {
-        delete[] Vconstraint;
-        Vconstraint = 0;
+    if (mLinearVelocities != 0) {
+        delete[] mLinearVelocities;
+        mLinearVelocities = 0;
     }
-    if (Wconstraint != 0) {
-        delete[] Wconstraint;
-        Wconstraint = 0;
-    }
-    if (V1 != 0) {
-        delete[] V1;
-        V1 = 0;
-    }
-    if (W1 != 0) {
-        delete[] W1;
-        W1 = 0;
+    if (mAngularVelocities != 0) {
+        delete[] mAngularVelocities;
+        mAngularVelocities = 0;
     }
 }
 
 // Set the number of iterations of the LCP solver
 inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
     mNbIterations = nbIterations;
-}         
-
-// Solve the current LCP problem
-inline void ConstraintSolver::solve(decimal dt) {
-    
-    // Initialize the solver
-    initialize();
-
-    initializeBodies();
-
-    // Fill-in all the matrices needed to solve the LCP problem
-    initializeContactConstraints(dt);
-
-    // Compute the matrix B
-    computeMatrixB_sp();
-
-    // Solve the LCP problem (computation of lambda)
-    solveLCP(dt);
-
-    // Cache the lambda values in order to use them in the next step
-    cacheLambda();
-    
-    // Compute the vector Vconstraint
-    computeVectorVconstraint(dt);
 }
 
 } // End of ReactPhysics3D namespace
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index b8d326c0..bd4b3c6d 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -123,15 +123,16 @@ void DynamicsWorld::updateAllBodiesMotion() {
                 newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
                 newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
             }
+            else {
+                // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
+                // external forces and torques.
+                newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
+                newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
 
-            // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
-            // external forces and torques.
-            newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
-            newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
-
-            // Add the velocity V1 to the new velocity
-            newLinearVelocity += rigidBody->getLinearVelocity();
-            newAngularVelocity += rigidBody->getAngularVelocity();
+                // Add the velocity V1 to the new velocity
+                newLinearVelocity += rigidBody->getLinearVelocity();
+                newAngularVelocity += rigidBody->getAngularVelocity();
+            }
             
             // Update the position and the orientation of the body according to the new velocity
             updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);