From 9677d4fbaecfb283ee5ec57e3e18391f8105b647 Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Mon, 3 May 2010 22:41:07 +0000
Subject: [PATCH] Continue to implement this class

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@313 92aac97c-a6ce-11dd-a772-7fcde58d38e6
---
 .../engine/ConstraintSolver.cpp               | 53 ++++++++++++-------
 .../reactphysics3d/engine/ConstraintSolver.h  | 11 ++--
 2 files changed, 39 insertions(+), 25 deletions(-)

diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp
index 9bb9399f..56ec4257 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.cpp
+++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp
@@ -36,10 +36,10 @@ ConstraintSolver::~ConstraintSolver() {
 
 // Allocate all the matrices needed to solve the LCP problem
 void ConstraintSolver::allocate() {
-    unsigned int nbConstraints = 0;
+    uint nbConstraints = 0;
 
     // For each constraint
-    for (unsigned int c=0; c<physicsWorld.getConstraints().size(); c++) {
+    for (uint c=0; c<physicsWorld.getConstraints().size(); c++) {
         Constraint* constraint = physicsWorld.getConstraints().at(c);
 
         // Evaluate the constraint
@@ -66,7 +66,7 @@ void ConstraintSolver::allocate() {
     nbBodies = bodyNumberMapping.size();
 
     bodyMapping = new Body**[nbConstraints];
-    for (unsigned int i=0; i<nbConstraints; i++) {
+    for (uint i=0; i<nbConstraints; i++) {
         bodyMapping[i] = new Body*[2];
     }
 
@@ -88,7 +88,7 @@ void ConstraintSolver::allocate() {
 void ConstraintSolver::fillInMatrices() {
 
     // For each active constraint
-    for (unsigned int c=0; c<activeConstraints.size(); c++) {
+    for (uint c=0; c<activeConstraints.size(); c++) {
         Constraint* constraint = activeConstraints.at(c);
 
         // Fill in the J_sp matrix
@@ -106,7 +106,7 @@ void ConstraintSolver::fillInMatrices() {
         // Fill in the error vector
         errorValues.fillInSubVector(c, constraint->getErrorValue());
 
-        unsigned int nbAuxConstraints = constraint->getNbAuxConstraints();
+        uint nbAuxConstraints = constraint->getNbAuxConstraints();
 
         // If the current constraint has auxiliary constraints
         if (nbAuxConstraints > 0) {
@@ -114,7 +114,7 @@ void ConstraintSolver::fillInMatrices() {
             J_sp.fillInSubMatrix(c+1, 0, constraint->getAuxJacobian());
 
             // For each auxiliary constraints
-            for (unsigned int i=1; i<nbAuxConstraints; i++) {
+            for (uint i=1; i<nbAuxConstraints; i++) {
                 // Fill in the body mapping matrix
                 bodyMapping[c+i][0] = constraint->getBody1();
                 bodyMapping[c+i][1] = constraint->getBody2();
@@ -127,9 +127,9 @@ void ConstraintSolver::fillInMatrices() {
     }
 
     // For each current body that is implied in some constraint
-    for (unsigned int b=0; b<nbBodies; b++) {
+    for (uint b=0; b<nbBodies; b++) {
         Body* body = constraintBodies.at(b);
-        unsigned int bodyNumber = bodyNumberMapping.at(body);
+        uint bodyNumber = bodyNumberMapping.at(body);
         
         // TODO : Use polymorphism and remove this casting
         RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
@@ -156,30 +156,43 @@ void ConstraintSolver::freeMemory() {
     bodyNumberMapping.clear();
 
     // Free the bodyMaping array
-    for (unsigned int i=0; i<nbBodies; i++) {
+    for (uint i=0; i<nbBodies; i++) {
         delete[] bodyMapping[i];
     }
     delete[] bodyMapping;
 }
 
 // Compute the vector b
-void ConstraintSolver::computeVectorB() {
+void ConstraintSolver::computeVectorB(double dt) {
+    uint indexBody1, indexBody2;
+    double oneOverDT = 1.0/dt;
     
+    b = errorValues * oneOverDT;
+
+    // Substract 1.0/dt*J*V to the vector b
+    for (uint c = 0; c<activeConstraints.size(); c++) {
+        indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
+        indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
+        b -= oneOverDT * (J_sp(c, 0) * V.getSubVector(indexBody1, 6));
+        b -= oneOverDT * (J_sp(c, 1) * V.getSubVector(indexBody2, 6));
+    }
+
+    // TODO : Continue to implement this method ... compute and remove J*Minv*Fext from b
 }
 
 // Compute the matrix B_sp
 void ConstraintSolver::computeMatrixB_sp() {
-    unsigned int indexBody1;
-    unsigned int indexBody2;
+    uint indexBody1;
+    uint indexBody2;
 
     // For each constraint
-    for (unsigned int j = 0; j<activeConstraints.size(); j++) {
-        indexBody1 = bodyNumberMapping[bodyMapping[j][0]];
-        indexBody2 = bodyNumberMapping[bodyMapping[j][1]];
-        Matrix b1 = Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6) * J_sp.getSubMatrix(j, 0, 1, 6).getTranspose();
-        Matrix b2 = Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6) * J_sp.getSubMatrix(j, 6, 1, 6).getTranspose();
-        B_sp.fillInSubMatrix(0, j, b1);
-        B_sp.fillInSubMatrix(6, j, b2);
+    for (uint c = 0; c<activeConstraints.size(); c++) {
+        indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
+        indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
+        Matrix b1 = Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6) * J_sp.getSubMatrix(c, 0, 1, 6).getTranspose();
+        Matrix b2 = Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6) * J_sp.getSubMatrix(c, 6, 1, 6).getTranspose();
+        B_sp.fillInSubMatrix(0, c, b1);
+        B_sp.fillInSubMatrix(6, c, b2);
     }
 }
 
@@ -192,7 +205,7 @@ void ConstraintSolver::solve(double dt) {
     fillInMatrices();
 
     // Compute the vector b
-    computeVectorB();
+    computeVectorB(double dt);
 
     // Compute the matrix B
     computeMatrixB_sp();
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h
index bb88158d..cb47ba8b 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.h
+++ b/sources/reactphysics3d/engine/ConstraintSolver.h
@@ -21,6 +21,7 @@
 #define CONSTRAINTSOLVER_H
 
 // Libraries
+#include "../typeDefinitions.h"
 #include "../constraint/Constraint.h"
 #include "PhysicsWorld.h"
 #include <map>
@@ -29,7 +30,7 @@
 namespace reactphysics3d {
 
 // Constants
-const unsigned int MAX_LCP_ITERATIONS = 10;     // Maximum number of iterations when solving a LCP problem
+const uint MAX_LCP_ITERATIONS = 10;     // Maximum number of iterations when solving a LCP problem
 
  /*  -------------------------------------------------------------------
     Class ConstrainSolver :
@@ -43,11 +44,11 @@ class ConstraintSolver {
         PhysicsWorld& physicsWorld;                                         // Reference to the physics world
         std::vector<Constraint*> activeConstraints;                         // Current active constraints in the physics world
         std::vector<Body*> constraintBodies;                                // Bodies that are implied in some constraint
-        unsigned int nbBodies;                                              // Current number of bodies in the physics world
-        std::map<Body*, unsigned int> bodyNumberMapping;                    // Map a body pointer with its index number
+        uint nbBodies;                                              // Current number of bodies in the physics world
+        std::map<Body*, uint> bodyNumberMapping;                            // Map a body pointer with its index number
         Body*** bodyMapping;                                                // 2-dimensional array that contains the mapping of body reference
                                                                             // 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
+                                                                            // the pointer to 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
@@ -63,7 +64,7 @@ class ConstraintSolver {
         void allocate();                                                    // 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
-        void computeVectorB();                                              // Compute the vector b
+        void computeVectorB(double dt);                                     // Compute the vector b
         void computeMatrixB_sp();                                           // Compute the matrix B_sp
 
     public: