diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 92d51c02..03a2bb66 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -114,9 +114,9 @@ void ConstraintSolver::allocate() {
         bodiesCapacity = nbBodies;
         
         Minv_sp = new Matrix6x6[nbBodies];
-        V1 = new Vector[nbBodies];
-        Vconstraint = new Vector[nbBodies];
-        Fext = new Vector[nbBodies];
+        V1 = new Vector6D[nbBodies];
+        Vconstraint = new Vector6D[nbBodies];
+        Fext = new Vector6D[nbBodies];
 
         avBodiesNumber = 0;
         avBodiesCounter = 0;
@@ -231,10 +231,6 @@ void ConstraintSolver::fillInMatrices() {
     // For each current body that is implied in some constraint
     RigidBody* rigidBody;
     Body* body;
-    Vector v(6);
-    Vector f(6);
-    //Matrix identity = Matrix::identity(3);
-    //Matrix mInv(6,6);
     uint b=0;
     for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
         body = *it;
@@ -245,20 +241,23 @@ void ConstraintSolver::fillInMatrices() {
         assert(rigidBody != 0);
         
         // Compute the vector V1 with initial velocities values
-        v.fillInSubVector(0, rigidBody->getLinearVelocity());
-        v.fillInSubVector(3, rigidBody->getAngularVelocity());
-        V1[bodyNumber].changeSize(6);
-        V1[bodyNumber] = v;
+        V1[bodyNumber].setValue(0, rigidBody->getLinearVelocity().getValue(0));
+        V1[bodyNumber].setValue(1, rigidBody->getLinearVelocity().getValue(1));
+        V1[bodyNumber].setValue(2, rigidBody->getLinearVelocity().getValue(2));
+        V1[bodyNumber].setValue(3, rigidBody->getAngularVelocity().getValue(0));
+        V1[bodyNumber].setValue(4, rigidBody->getAngularVelocity().getValue(1));
+        V1[bodyNumber].setValue(5, rigidBody->getAngularVelocity().getValue(2));
 
         // Compute the vector Vconstraint with final constraint velocities
-        Vconstraint[bodyNumber].changeSize(6);
         Vconstraint[bodyNumber].initWithValue(0.0);
         
         // Compute the vector with forces and torques values
-        f.fillInSubVector(0, rigidBody->getExternalForce());
-        f.fillInSubVector(3, rigidBody->getExternalTorque());
-        Fext[bodyNumber].changeSize(6);
-        Fext[bodyNumber] = f;
+        Fext[bodyNumber].setValue(0, rigidBody->getExternalForce().getValue(0));
+        Fext[bodyNumber].setValue(1, rigidBody->getExternalForce().getValue(1));
+        Fext[bodyNumber].setValue(2, rigidBody->getExternalForce().getValue(2));
+        Fext[bodyNumber].setValue(3,  rigidBody->getExternalTorque().getValue(0));
+        Fext[bodyNumber].setValue(4,  rigidBody->getExternalTorque().getValue(1));
+        Fext[bodyNumber].setValue(5,  rigidBody->getExternalTorque().getValue(2));
 
         // Compute the inverse sparse mass matrix
         Minv_sp[bodyNumber].initWithValue(0.0);
@@ -276,11 +275,7 @@ void ConstraintSolver::fillInMatrices() {
             Minv_sp[bodyNumber].setValue(5, 3, tensorInv.getValue(2, 0));
             Minv_sp[bodyNumber].setValue(5, 4, tensorInv.getValue(2, 1));
             Minv_sp[bodyNumber].setValue(5, 5, tensorInv.getValue(2, 2));
-            //mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity);
-            //mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld());
         }
-        //Minv_sp[bodyNumber].changeSize(6, 6);
-        //Minv_sp[bodyNumber] = mInv;
     }
 }
 
@@ -295,12 +290,12 @@ void ConstraintSolver::computeVectorB(double dt) {
         // Substract 1.0/dt*J*V to the vector b
         indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
-        b.setValue(c, b.getValue(c) - (Matrix(J_sp[c][0]) * V1[indexBody1]).getValue(0,0) * oneOverDT); // TODO : Remove conversion here
-        b.setValue(c, b.getValue(c) - (Matrix(J_sp[c][1]) * V1[indexBody2]).getValue(0,0) * oneOverDT);
+        b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]) * oneOverDT);
+        b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]) * oneOverDT);
 
         // Substract J*M^-1*F_ext to the vector b
-        b.setValue(c, b.getValue(c) - ((Matrix(J_sp[c][0]) * Matrix(Minv_sp[indexBody1])) * Fext[indexBody1]
-                 + (Matrix(J_sp[c][1]) * Matrix(Minv_sp[indexBody2]))*Fext[indexBody2]).getValue(0,0));         // TODO : Delete conversion here
+        b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
+                 + (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]));
     }
 }
 
@@ -312,8 +307,6 @@ void ConstraintSolver::computeMatrixB_sp() {
     for (uint c = 0; c<nbConstraints; c++) {
         indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
-        //B_sp[0][c].changeSize(6,1);
-        //B_sp[1][c].changeSize(6,1);
         B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
         B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
     }
@@ -332,8 +325,8 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
     for (uint i=0; i<nbConstraints; i++) {
         indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
-        Vconstraint[indexBody1] = Vconstraint[indexBody1] + (Matrix(B_sp[0][i]) * lambda.getValue(i)).getVector() * dt;
-        Vconstraint[indexBody2] = Vconstraint[indexBody2] + (Matrix(B_sp[1][i]) * lambda.getValue(i)).getVector() * dt; // TODO : Remove conversion here
+        Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[0][i] * lambda.getValue(i)) * dt;
+        Vconstraint[indexBody2] = Vconstraint[indexBody2] + (B_sp[1][i] * lambda.getValue(i)) * dt;
     }
 }
 
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index ba58217e..e7d5760f 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -87,10 +87,10 @@ class ConstraintSolver {
         Vector upperBounds;                                     // Vector that contains the high limits for the variables of the LCP problem
         Matrix6x6* Minv_sp;                                        // Sparse representation of the Matrix that contains information about mass and inertia of each body
                                                                 // This is an array of size nbBodies that contains in each cell a 6x6 matrix
-        Vector* V1;                                             // Array that contains for each body the Vector that contains linear and angular velocities
+        Vector6D* V1;                                             // Array that contains for each body the Vector that contains linear and angular velocities
                                                                 // Each cell contains a 6x1 vector with linear and angular velocities
-        Vector* Vconstraint;                                    // Same kind of vector as V1 but contains the final constraint velocities
-        Vector* Fext;                                           // Array that contains for each body the vector that contains external forces and torques
+        Vector6D* Vconstraint;                                    // Same kind of vector as V1 but contains the final constraint velocities
+        Vector6D* Fext;                                           // Array that contains for each body the vector that contains external forces and torques
                                                                 // Each cell contains a 6x1 vector with external force and torque.
         void initialize();                                      // Initialize the constraint solver before each solving
         void allocate();                                        // Allocate all the memory needed to solve the LCP problem
@@ -122,7 +122,7 @@ inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
 // Return the constrained linear velocity of a body after solving the LCP problem
 inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
     assert(isConstrainedBody(body));
-    Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(0, 3);
+    const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
     return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
 
 }
@@ -130,8 +130,8 @@ inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body)
 // Return the constrained angular velocity of a body after solving the LCP problem
 inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
     assert(isConstrainedBody(body));
-    Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(3, 3);
-    return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
+    const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
+    return Vector3D(vec.getValue(3), vec.getValue(4), vec.getValue(5));
 }
 
 // Cleanup of the constraint solver
diff --git a/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp b/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp
index bba31a17..daa68a9a 100644
--- a/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp
+++ b/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp
@@ -48,23 +48,20 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
     double lambdaTemp;
     uint i, iter;
     Vector6D* a = new Vector6D[nbBodies];           // Array that contains nbBodies vector of dimension 6x1
-    //for (i=0; i<nbBodies; i++) {
-    //    a[i].changeSize(6);
-    //}
 
     // Compute the vector a
     computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
 
     // For each constraint
     for (i=0; i<nbConstraints; i++) {
-        d[i] = (Matrix(J_sp[i][0]) * B_sp[0][i] + Matrix(J_sp[i][1]) * B_sp[1][i]).getValue(0,0);   // TODO : Remove conversion here
+        d[i] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]);
     }
 
     for(iter=0; iter<maxIterations; iter++) {
         for (i=0; i<nbConstraints; i++) {
             indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
             indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
-            deltaLambda = (b.getValue(i) - (Matrix(J_sp[i][0]) * a[indexBody1]).getValue(0,0) - (Matrix(J_sp[i][1]) * a[indexBody2]).getValue(0,0)) / d[i]; // TODO : Remove conversion here
+            deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]) - (J_sp[i][1] * a[indexBody2])) / d[i];
             lambdaTemp = lambda.getValue(i);
             lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
             deltaLambda = lambda.getValue(i) - lambdaTemp;