mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-04-14 21:53:06 +00:00
parent ba60a97dd6
commit 3a13c0ebd6
6 changed files with 65 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -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

2
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -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

21
sources/reactphysics3d/mathematics/Matrix.cpp
View File

@ -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

2
sources/reactphysics3d/mathematics/Matrix.h
View File

@ -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

1
sources/reactphysics3d/mathematics/Vector.h
View File

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

1
sources/reactphysics3d/mathematics/Vector3D.h
View File

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