From 5f2b65a8b8d57cab4a0d6e47840bc008271940b1 Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Mon, 19 Jul 2010 19:34:35 +0000
Subject: [PATCH] git-svn-id:
 https://reactphysics3d.googlecode.com/svn/trunk@360
 92aac97c-a6ce-11dd-a772-7fcde58d38e6

---
 .../mathematics/lcp/LCPProjectedGaussSeidel.cpp           | 8 +++++---
 .../mathematics/lcp/LCPProjectedGaussSeidel.h             | 4 ++--
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp b/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp
index 852fae11..d16bef68 100644
--- a/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp
+++ b/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp
@@ -40,9 +40,8 @@ void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstra
                                     uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
                                     const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
 
-    int size1 = lambda.getNbComponent();
-    int size2 = lambdaInit.getNbComponent();
     lambda = lambdaInit;
+
     double* d = new double[nbConstraints];         // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
     uint indexBody1, indexBody2;
     double deltaLambda;
@@ -53,6 +52,9 @@ void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstra
         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] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]).getValue(0,0);
@@ -79,7 +81,7 @@ void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstra
 // Compute the vector a used in the solve() method
 // Note that a = B * lambda
 void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
-                                             const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
+                                             Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
                                              Vector* const a, uint nbBodies) const {
     uint i;
     uint indexBody1, indexBody2;
diff --git a/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h b/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h
index e51d4d3d..5f5efd4b 100644
--- a/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h
+++ b/sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h
@@ -37,8 +37,8 @@ class LCPProjectedGaussSeidel : public LCPSolver {
     protected: 
 
         void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
-                                                     const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
-                                                     Vector* const a, uint nbBodies) const ;                                                       // Compute the vector a used in the solve() method
+                            Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
+                            Vector* const a, uint nbBodies) const ;                                                       // Compute the vector a used in the solve() method
 
     public:
         LCPProjectedGaussSeidel(uint maxIterations);                                                                    // Constructor