From decc06037a71e09cb9e53f6401af631803ee4812 Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Sun, 6 Jun 2010 21:18:49 +0000
Subject: [PATCH] git-svn-id:
 https://reactphysics3d.googlecode.com/svn/trunk@326
 92aac97c-a6ce-11dd-a772-7fcde58d38e6

---
 .../engine/ConstraintSolver.cpp               | 35 +++++++++++++++++--
 .../reactphysics3d/engine/ConstraintSolver.h  |  1 +
 .../reactphysics3d/engine/PhysicsEngine.cpp   |  2 +-
 3 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp
index 387466cb..d2568cb1 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.cpp
+++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp
@@ -205,8 +205,7 @@ void ConstraintSolver::computeVectorB(double dt) {
 
 // Compute the matrix B_sp
 void ConstraintSolver::computeMatrixB_sp() {
-    uint indexBody1;
-    uint indexBody2;
+    uint indexBody1, indexBody2;
 
     // For each constraint
     for (uint c = 0; c<activeConstraints.size(); c++) {
@@ -217,6 +216,29 @@ void ConstraintSolver::computeMatrixB_sp() {
     }
 }
 
+// Compute the vector V2 according to the formula
+// V2 = dt * (M^-1 * J^T * lambda + M^-1 * F_ext) + V1
+// Note that we use the vector V to store both V1 and V2 and that at the beginning
+// of this method, the vector V already contains the vector V1.
+// Note that M^-1 * J^T = B.
+// This method is called after that the LCP solver have computed lambda
+void ConstraintSolver::computeVectorV(double dt) {
+    uint indexBody1, indexBody2;
+
+    // Compute dt * (M^-1 * J^T * lambda
+    for (uint i=0; i<activeConstraints.size(); i++) {
+        indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
+        indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
+        V[indexBody1] = V[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
+        V[indexBody2] = V[indexBody2] + (B_sp[indexBody2][i] * lambda.getValue(i)).getVector() * dt;
+    }
+
+    // Compute dt * (M^-1 * F_ext)
+    for (uint i=0; i<nbBodies; i++) {
+        V[i] = V[i] + (Minv_sp[i] * Fext[i]).getVector() * dt;
+    }
+}
+
 // Solve the current LCP problem
 void ConstraintSolver::solve(double dt) {
     // Allocate memory for the matrices
@@ -234,7 +256,14 @@ void ConstraintSolver::solve(double dt) {
     // Solve the LCP problem (computation of lambda)
     lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
 
-    // TODO : Implement this method ...
+    // Compute the vector V2
+    computeVectorV(dt);
+
+    // Update the velocity of each body
+    // TODO : Put this code somewhere else
+    for (int i=0; i<nbBodies; i++) {
+        constraintBodies.at(i);
+    }
 
     freeMemory();
 }
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h
index 6d741df2..d28bcf94 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.h
+++ b/sources/reactphysics3d/engine/ConstraintSolver.h
@@ -73,6 +73,7 @@ class ConstraintSolver {
         void freeMemory();                                      // Free the memory that was allocated in the allocate() method
         void computeVectorB(double dt);                         // Compute the vector b
         void computeMatrixB_sp();                               // Compute the matrix B_sp
+        void computeVectorV(double dt);                         // Compute the vector V2
 
     public:
         ConstraintSolver(PhysicsWorld& world);                  // Constructor
diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp
index 45af4249..c87b4682 100644
--- a/sources/reactphysics3d/engine/PhysicsEngine.cpp
+++ b/sources/reactphysics3d/engine/PhysicsEngine.cpp
@@ -132,7 +132,7 @@ void PhysicsEngine::updateBodyState(RigidBody* const rigidBody, const Time& time
     // If the gravity force is on
     if(world->getIsGravityOn()) {
         // Apply the current gravity force to the body
-        rigidBody->getCurrentBodyState().setForce(world->getGravity());
+        rigidBody->getCurrentBodyState().setExternalForce(world->getGravity());
     }
 
     // The current body state of the body becomes the previous body state