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 // Compute the vector b
void ConstraintSolver::computeVectorB() { void ConstraintSolver::computeVectorB() {
// TODO : Implement this method ...
} }
// Compute the matrix B_sp // Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_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 // 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 std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
unsigned int nbBodies; // Current number of bodies in the physics world 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 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 // 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 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 // 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 // Static function that return a identity matrix of size nxn
Matrix Matrix::identity(int dimension) throw(std::invalid_argument) { Matrix Matrix::identity(int dimension) throw(std::invalid_argument) {
// Argument verification // Argument verification

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

@ -39,36 +39,38 @@ namespace reactphysics3d {
class Matrix { class Matrix {
private : private :
unsigned int nbRow; // Number of row in the matrix unsigned int nbRow; // Number of row in the matrix
unsigned int nbColumn; // Number of colum in the matrix unsigned int nbColumn; // Number of colum in the matrix
double** array; // Dynamic array that contains the values of the matrix double** array; // Dynamic array that contains the values of the matrix
public : public :
Matrix(); // Constructor without argument Matrix(); // Constructor without argument
Matrix(int nbRow, int nbColum) throw(std::invalid_argument); // Constructor of the class Matrix 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 Matrix& matrix); // Copy constructor of the class Matrix
Matrix(const Matrix3x3& matrix); // Conversion from Matrix3x3 Matrix(const Matrix3x3& matrix); // Conversion from Matrix3x3
virtual ~Matrix(); // Destructor of the class Matrix virtual ~Matrix(); // Destructor of the class Matrix
double getValue(int i, int j) const throw(std::invalid_argument); // Return a value in the 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 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 getNbRow() const; // Return the number of row of the matrix
int getNbColumn() const; // Return the number of column 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 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 getTranspose() const; // Return the transposed matrixs
Matrix getInverse() const throw(MathematicsException); // Return the inverse of the matrix if there exists 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 getDeterminant() const throw(MathematicsException); // Return the determinant of the matrix
double getTrace() const throw(MathematicsException); // Return the trace of a square 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 Matrix getSubMatrix(unsigned int i, unsigned int j,
void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix); // Fill in a sub-matrix of the current matrix with another matrix unsigned int nbRows, unsigned int nbColumns) const throw(std::invalid_argument); // Return a sub matrix of size of the current matrix
void initWithValue(double value); // Initialize all the matrix with the given value static Matrix identity(int dimension) throw(std::invalid_argument); // Return the identity matrix I of the given dimension
void display() const; // TO DELETE 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 --- // // --- 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 addition
Matrix operator-(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for substraction 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*(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) const throw(MathematicsException); // Overloaded operator for multiplication with a matrix
Matrix& operator=(const Matrix& matrix2) throw(MathematicsException); // Overloaded operator for assignment Matrix& operator=(const Matrix& matrix2) throw(MathematicsException); // Overloaded operator for assignment
bool operator==(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for equality condition bool operator==(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for equality condition
}; };
// Function to get a value in the matrix (inline) // Function to get a value in the matrix (inline)

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

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

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

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