From 3a13c0ebd6b6a272a666411c61fae46b27764a02 Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Wed, 14 Apr 2010 21:53:06 +0000
Subject: [PATCH] git-svn-id:
 https://reactphysics3d.googlecode.com/svn/trunk@308
 92aac97c-a6ce-11dd-a772-7fcde58d38e6

---
 .../engine/ConstraintSolver.cpp               | 15 +++++-
 .../reactphysics3d/engine/ConstraintSolver.h  |  2 +-
 sources/reactphysics3d/mathematics/Matrix.cpp | 21 ++++++++
 sources/reactphysics3d/mathematics/Matrix.h   | 54 ++++++++++---------
 sources/reactphysics3d/mathematics/Vector.h   |  1 +
 sources/reactphysics3d/mathematics/Vector3D.h |  1 +
 6 files changed, 65 insertions(+), 29 deletions(-)

diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp
index 0403d86c..9bb9399f 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.cpp
+++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp
@@ -164,12 +164,23 @@ void ConstraintSolver::freeMemory() {
 
 // Compute the vector b
 void ConstraintSolver::computeVectorB() {
-    // TODO : Implement this method ...
+    
 }
 
 // Compute the matrix B_sp
 void ConstraintSolver::computeMatrixB_sp() {
-    // TODO : Implement this method ...
+    unsigned int indexBody1;
+    unsigned int 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);
+    }
 }
 
 // Solve the current LCP problem
diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h
index 9d53c244..bb88158d 100644
--- a/sources/reactphysics3d/engine/ConstraintSolver.h
+++ b/sources/reactphysics3d/engine/ConstraintSolver.h
@@ -45,7 +45,7 @@ class ConstraintSolver {
         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
-        Body** bodyMapping;                                                 // 2-dimensional array that contains the mapping of body reference
+        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
                                                                             // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
diff --git a/sources/reactphysics3d/mathematics/Matrix.cpp b/sources/reactphysics3d/mathematics/Matrix.cpp
index 6a7699ea..ff85812e 100644
--- a/sources/reactphysics3d/mathematics/Matrix.cpp
+++ b/sources/reactphysics3d/mathematics/Matrix.cpp
@@ -266,6 +266,27 @@ double Matrix::getTrace() const throw(MathematicsException) {
 
 }
 
+// Return a sub matrix of size of the current matrix
+Matrix Matrix::getSubMatrix(unsigned int i, unsigned int j,
+                                   unsigned int sizeRows, unsigned int sizeColumns) const throw(std::invalid_argument) {
+    // Check the arguments
+    if (i<0 || j<0 || i+sizeRows >= nbRow || j+sizeColumns >= nbColumn) {
+        // Throw an exception
+        throw std::invalid_argument("Error : The arguments are out of matrix bounds");
+    }
+
+    // Compute the sub-matrix
+    Matrix resultMatrix(sizeRows, sizeColumns);
+    for (unsigned int k=0; k<sizeRows; k++) {
+        for (unsigned int l=0; l<sizeColumns; l++) {
+            resultMatrix.array[k][l] = array[i+k][j+l];
+        }
+    }
+
+    // Return the sub-matrix
+    return resultMatrix;
+}
+
 // Static function that return a identity matrix of size nxn
 Matrix Matrix::identity(int dimension) throw(std::invalid_argument) {
     // Argument verification
diff --git a/sources/reactphysics3d/mathematics/Matrix.h b/sources/reactphysics3d/mathematics/Matrix.h
index 54065459..8cc623b5 100644
--- a/sources/reactphysics3d/mathematics/Matrix.h
+++ b/sources/reactphysics3d/mathematics/Matrix.h
@@ -39,36 +39,38 @@ namespace reactphysics3d {
 class Matrix {
     private :
         unsigned int nbRow;                                                             // Number of row in the matrix
-        unsigned int nbColumn;                                                          // Number of colum in the matrix
-        double** array;                                                                 // Dynamic array that contains the values of the matrix
+        unsigned int nbColumn;                                                                                          // Number of colum in the matrix
+        double** array;                                                                                                 // Dynamic array that contains the values of the matrix
 
     public :
-        Matrix();                                                                       // Constructor without argument
-        Matrix(int nbRow, int nbColum) throw(std::invalid_argument);                    // Constructor of the class Matrix
-        Matrix(const Matrix& matrix);                                                   // Copy constructor of the class Matrix
-        Matrix(const Matrix3x3& matrix);                                                // Conversion from Matrix3x3
-        virtual ~Matrix();                                                              // Destructor of the class Matrix
-        double getValue(int i, int j) const throw(std::invalid_argument);               // Return a value in the matrix
-        void setValue(int i, int j, double value) throw(std::invalid_argument);         // Set a value in the matrix
-        int getNbRow() const;                                                           // Return the number of row of the matrix
-        int getNbColumn() const;                                                        // Return the number of column of the matrix
-        Matrix getCofactor(int i, int j) const throw(std::invalid_argument);            // Return the cofactor matrix by removing row i and column j
-        Matrix getTranspose() const;                                                    // Return the transposed matrixs
-        Matrix getInverse() const throw(MathematicsException);                          // Return the inverse of the matrix if there exists
-        double getDeterminant() const throw(MathematicsException);                      // Return the determinant of the matrix
-        double getTrace() const throw(MathematicsException);                            // Return the trace of a square matrix
-        static Matrix identity(int dimension) throw(std::invalid_argument);             // Return the identity matrix I of the given dimension
-        void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix);  // Fill in a sub-matrix of the current matrix with another matrix
-        void initWithValue(double value);                                               // Initialize all the matrix with the given value
-        void display() const;                                                           // TO DELETE
+        Matrix();                                                                                                       // Constructor without argument
+        Matrix(int nbRow, int nbColum) throw(std::invalid_argument);                                                    // Constructor of the class Matrix
+        Matrix(const Matrix& matrix);                                                                                   // Copy constructor of the class Matrix
+        Matrix(const Matrix3x3& matrix);                                                                                // Conversion from Matrix3x3
+        virtual ~Matrix();                                                                                              // Destructor of the class Matrix
+        double getValue(int i, int j) const throw(std::invalid_argument);                                               // Return a value in the matrix
+        void setValue(int i, int j, double value) throw(std::invalid_argument);                                         // Set a value in the matrix
+        int getNbRow() const;                                                                                           // Return the number of row of the matrix
+        int getNbColumn() const;                                                                                        // Return the number of column of the matrix
+        Matrix getCofactor(int i, int j) const throw(std::invalid_argument);                                            // Return the cofactor matrix by removing row i and column j
+        Matrix getTranspose() const;                                                                                    // Return the transposed matrixs
+        Matrix getInverse() const throw(MathematicsException);                                                          // Return the inverse of the matrix if there exists
+        double getDeterminant() const throw(MathematicsException);                                                      // Return the determinant of the matrix
+        double getTrace() const throw(MathematicsException);                                                            // Return the trace of a square matrix
+        Matrix getSubMatrix(unsigned int i, unsigned int j,
+                            unsigned int nbRows, unsigned int nbColumns) const throw(std::invalid_argument);            // Return a sub matrix of size of the current matrix
+        static Matrix identity(int dimension) throw(std::invalid_argument);                                             // Return the identity matrix I of the given dimension
+        void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix);                                  // Fill in a sub-matrix of the current matrix with another matrix
+        void initWithValue(double value);                                                                               // Initialize all the matrix with the given value
+        void display() const;                                                                                           // TO DELETE
 
         // --- Overloaded operators --- //
-        Matrix operator+(const Matrix& matrix2) const throw(MathematicsException);      // Overloaded operator for addition
-        Matrix operator-(const Matrix& matrix2) const throw(MathematicsException);      // Overloaded operator for substraction
-        Matrix operator*(double nb) const;                                              // Overloaded operator for multiplication with a number
-        Matrix operator*(const Matrix& matrix2) const throw(MathematicsException);      // Overloaded operator for multiplication with a matrix
-        Matrix& operator=(const Matrix& matrix2) throw(MathematicsException);           // Overloaded operator for assignment
-        bool operator==(const Matrix& matrix2) const throw(MathematicsException);       // Overloaded operator for equality condition
+        Matrix operator+(const Matrix& matrix2) const throw(MathematicsException);                                      // Overloaded operator for addition
+        Matrix operator-(const Matrix& matrix2) const throw(MathematicsException);                                      // Overloaded operator for substraction
+        Matrix operator*(double nb) const;                                                                              // Overloaded operator for multiplication with a number
+        Matrix operator*(const Matrix& matrix2) const throw(MathematicsException);                                      // Overloaded operator for multiplication with a matrix
+        Matrix& operator=(const Matrix& matrix2) throw(MathematicsException);                                           // Overloaded operator for assignment
+        bool operator==(const Matrix& matrix2) const throw(MathematicsException);                                       // Overloaded operator for equality condition
 };
 
 // Function to get a value in the matrix (inline)
diff --git a/sources/reactphysics3d/mathematics/Vector.h b/sources/reactphysics3d/mathematics/Vector.h
index dada8095..5eea36c5 100644
--- a/sources/reactphysics3d/mathematics/Vector.h
+++ b/sources/reactphysics3d/mathematics/Vector.h
@@ -26,6 +26,7 @@
 #include <cassert>
 #include <iostream>
 
+
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
diff --git a/sources/reactphysics3d/mathematics/Vector3D.h b/sources/reactphysics3d/mathematics/Vector3D.h
index 691ab8d2..029624c3 100644
--- a/sources/reactphysics3d/mathematics/Vector3D.h
+++ b/sources/reactphysics3d/mathematics/Vector3D.h
@@ -26,6 +26,7 @@
 #include "exceptions.h"
 #include "mathematics_functions.h"
 
+
 // ReactPhysics3D namespace
 namespace reactphysics3d {