git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@308 92aac97c-a6ce-11dd-a772-7fcde58d38e6

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

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

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

sources/reactphysics3d/mathematics/Matrix.h

@@ -57,6 +57,8 @@ class Matrix {
         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

sources/reactphysics3d/mathematics/Vector.h

@@ -26,6 +26,7 @@
 #include <cassert>
 #include <iostream>
 
+
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 

sources/reactphysics3d/mathematics/Vector3D.h

@@ -26,6 +26,7 @@
 #include "exceptions.h"
 #include "mathematics_functions.h"
 
+
 // ReactPhysics3D namespace
 namespace reactphysics3d {