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

Correction of some errors

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@325 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-06-05 22:22:06 +00:00
parent 2f9a12f887
commit 639e91b21b
12 changed files with 105 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,7 +26,7 @@ using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
:physicsWorld(world), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
:physicsWorld(world), bodyMapping(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
}
@ -38,10 +38,12 @@ ConstraintSolver::~ConstraintSolver() {
// Allocate all the matrices needed to solve the LCP problem
void ConstraintSolver::allocate() {
uint nbConstraints = 0;
Constraint* constraint;
// For each constraint
for (uint c=0; c<physicsWorld.getConstraints().size(); c++) {
Constraint* constraint = physicsWorld.getConstraints().at(c);
std::vector<Constraint*>::iterator it;
for (it = physicsWorld.getConstraintsBeginIterator(); it <physicsWorld.getConstraintsEndIterator(); it++) {
constraint = *it;
// Evaluate the constraint
constraint->evaluate();
@ -230,7 +232,7 @@ void ConstraintSolver::solve(double dt) {
computeMatrixB_sp();
// Solve the LCP problem (computation of lambda)
//lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// TODO : Implement this method ...

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

@ -41,8 +41,8 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol
*/
class ConstraintSolver {
protected:
LCPSolver& lcpSolver; // LCP Solver
PhysicsWorld& physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
uint nbBodies; // Current number of bodies in the physics world
@ -75,7 +75,7 @@ class ConstraintSolver {
void computeMatrixB_sp(); // Compute the matrix B_sp
public:
ConstraintSolver(const PhysicsWorld& world); // Constructor
ConstraintSolver(PhysicsWorld& world); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem
};

2
sources/reactphysics3d/engine/PhysicsEngine.cpp
View File

@ -91,7 +91,7 @@ void PhysicsEngine::updateCollision() {
if (collisionDetection.computeCollisionDetection()) {
// TODO : Delete this ----------------------------------------------------------
for (std::vector<Constraint*>::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) {
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>((*it)->getBody1());
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>((*it)->getBody2());
rigidBody1->setIsMotionEnabled(false);

26
sources/reactphysics3d/engine/PhysicsWorld.h
View File

@ -59,10 +59,10 @@ class PhysicsWorld {
void addConstraint(Constraint* constraint) throw(std::invalid_argument); // Add a constraint
void removeConstraint(Constraint* constraint) throw(std::invalid_argument); // Remove a constraint
void removeAllContactConstraints(); // Remove all collision contacts constraints
std::vector<Constraint*>::const_iterator getConstraintListStartIterator() const; // Return a start iterator on the constraint list
std::vector<Constraint*>::const_iterator getConstraintListEndIterator() const; // Return a end iterator on the constraint list
std::vector<Body*>& getBodies(); // Return a reference to the bodies of the physics world
std::vector<Constraint*>& getConstraints(); // Return a reference to the constraints of the physics world
std::vector<Constraint*>::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list
std::vector<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world
std::vector<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world
};
// --- Inline functions --- //
@ -95,25 +95,25 @@ inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) {
}
// Return a start iterator on the constraint list
inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListStartIterator() const {
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsBeginIterator() {
return constraints.begin();
}
// Return a end iterator on the constraint list
inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListEndIterator() const {
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsEndIterator() {
return constraints.end();
}
// Return a reference to the bodies of the physics world
std::vector<Body*>& PhysicsWorld::getBodies() {
return bodies;
// Return an iterator to the beginning of the bodies of the physics world
inline std::vector<Body*>::iterator PhysicsWorld::getBodiesBeginIterator() {
return bodies.begin();
}
// Return a reference to the constraints of the physics world
std::vector<Constraint*>& PhysicsWorld::getConstraints() {
return constraints;
// Return an iterator to the end of the bodies of the physics world
inline std::vector<Body*>::iterator PhysicsWorld::getBodiesEndIterator() {
return bodies.end();
}
} // End of the ReactPhysics3D namespace
#endif
#endif

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

@ -276,7 +276,6 @@ double Matrix::getTrace() const throw(MathematicsException) {
// We throw an exception because the matrix is non-square
throw MathematicsException("MathematicsException : Impossible to compute the trace for a non-square matrix");
}
}
// Return a sub matrix of size of the current matrix
@ -353,6 +352,16 @@ void Matrix::initWithValue(double value) {
}
}
Vector Matrix::getVector() const {
assert(nbColumn == 1);
Vector vec(nbRow);
for (int i=0; i<nbRow; i++) {
vec.setValue(i, array[i][0]);
}
return vec;
}
// Definition of the operator + for the sum of two matrices with references
Matrix Matrix::operator+(const Matrix& matrix2) const throw(MathematicsException) {
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
@ -440,6 +449,27 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException
}
}
// Overloaded operator for multiplication with a vector
Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) {
// Check the sizes of the matrices
if (nbColumn == vector.getNbComponent()) {
Matrix result(nbColumn, 1);
for (int i=0; i<nbColumn; i++) {
double sum = 0.0;
for (int j=0; j<vector.getNbComponent(); j++) {
sum += array[i][j] * vector.getValue(j);
}
result.array[i][0] = sum;
}
return result;
}
else {
// Throw an exception because the multiplication is impossible
throw MathematicsException("MathematicsException : The sizes of the matrix and the vector aren't compatible for the multiplication");
}
}
// Overloaded operator = for the assignment
Matrix& Matrix::operator=(const Matrix& matrix2) throw(MathematicsException) {

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

@ -61,6 +61,7 @@ class 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
Vector getVector() const;
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
@ -71,6 +72,7 @@ class Matrix {
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 Vector& vector) const throw(MathematicsException); // Overloaded operator for multiplication with a vector
Matrix& operator=(const Matrix& matrix2) throw(MathematicsException); // Overloaded operator for assignment
bool operator==(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for equality condition
};

8
sources/reactphysics3d/mathematics/Vector.cpp
View File

@ -100,6 +100,14 @@ Vector Vector::getUnit() const throw(MathematicsException) {
}
}
void Vector::setVector(const Vector& vector) {
assert(nbComponent == vector.nbComponent);
for (int i=0; i<nbComponent; i++) {
tab[i] = vector.tab[i];
}
}
// Method to compute the scalar product of two vectors
double Vector::scalarProduct(const Vector& vector) const throw(MathematicsException) {
// Check the sizes of the two vectors

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

@ -59,14 +59,16 @@ class Vector {
Vector crossProduct(const Vector& vector) const throw(MathematicsException); // Cross product of two vectors (in 3D only)
void fillInSubVector(uint index, const Vector& subVector); // Replace a part of the current vector with another sub-vector
Vector getSubVector(uint index, uint nbElements) const throw(std::invalid_argument); // Return a sub-vector of the current vector
void setVector(const Vector& vector);
bool isUnit() const; // Return true if the vector is unit and false otherwise
void changeSize(uint newSize);
// --- Overloaded operators --- //
Vector operator+(const Vector& vector) const throw(MathematicsException); // Overloaded operator for addition
Vector operator-(const Vector& vector) const throw(MathematicsException); // Overloaded operator for substraction
Vector operator*(double number) const; // Overloaded operator for multiplication with a number
Vector& operator=(const Vector& vector) throw(MathematicsException); // Overloaded operator for the assignement to a Vector
bool operator==(const Vector& vector) const throw(MathematicsException); // Overloaded operator for the equality condition
Vector operator+(const Vector& vector) const throw(MathematicsException); // Overloaded operator for addition
Vector operator-(const Vector& vector) const throw(MathematicsException); // Overloaded operator for substraction
Vector operator*(double number) const; // Overloaded operator for multiplication with a number
Vector& operator=(const Vector& vector) throw(MathematicsException); // Overloaded operator for the assignement to a Vector
bool operator==(const Vector& vector) const throw(MathematicsException); // Overloaded operator for the equality condition
};
@ -162,6 +164,17 @@ inline Vector operator*(double number, const Vector& vector) {
return vector * number;
}
void Vector::changeSize(uint newSize) {
delete[] tab;
nbComponent = newSize;
tab = new double[nbComponent];
// Fill the array with the value of the vector
for (int i=0; i<nbComponent; ++i) {
tab[i] = 0.0;
}
}
} // End of the ReactPhysics3D namespace
#endif

27
sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp
View File

@ -36,16 +36,19 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** const B_sp, uint nbConstraints,
uint nbBodies, const Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
lambda = lambdaInit;
double d[] = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
Body* indexBody1, indexBody2;
double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
uint indexBody1, indexBody2;
double deltaLambda;
double lambdaTemp;
uint i, iter;
Vector* a = new Vector(6)[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
Vector* a = new Vector[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
for (i=0; i<nbBodies; i++) {
a[i].changeSize(6);
}
// For each constraint
for (i=0; i<nbConstraints; i++) {
@ -56,12 +59,12 @@ void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** co
for (i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
deltaLambda = (b(i) - J_sp[i][0]*a[indexBody1] - J_sp[i][1]*a[indexBody2]) / d[i];
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]).getValue(0,0) - (J_sp[i][1] * a[indexBody2]).getValue(0,0)) / d[i];
lambdaTemp = lambda.getValue(i);
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
deltaLambda = lambda.getValue(i) - lambdaTemp;
a[indexBody1] = a[indexBody1] + deltaLambda * B_sp[0][i];
a[indexBody2] = a[indexBody2] + deltaLambda * B_sp[1][i];
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda).getVector();
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda).getVector();
}
}
@ -72,11 +75,11 @@ void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** co
// Compute the vector a used in the solve() method
// Note that a = B * lambda
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, const Body*** const bodyMapping,
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
Vector* const a, uint nbBodies) const {
uint i;
Body* indexBody1, indexBody2;
uint indexBody1, indexBody2;
// Init the vector a with zero values
for (i=0; i<nbBodies; i++) {
@ -86,8 +89,8 @@ void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstr
for(i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
a[indexBody1] = a[indexBody1] + B_sp[0][i] * lambda(i);
a[indexBody2] = a[indexBody2] + B_sp[1][i] * lambda(i);
a[indexBody1] = a[indexBody1] + (B_sp[0][i].getVector() * lambda.getValue(i));
a[indexBody2] = a[indexBody2] + (B_sp[1][i].getVector() * lambda.getValue(i));
}
}

8
sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h
View File

@ -36,16 +36,16 @@ namespace reactphysics3d {
class LCPProjectedGaussSeidel : public LCPSolver {
protected:
void computeVectorA(const Vector& lambda, uint nbConstraints, const Body*** const bodyMapping,
void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
Vector* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method
public:
LCPProjectedGaussSeidel(uint maxIterations); // Constructor
virtual ~LCPProjectedGaussSeidel(); // Destructor
virtual void solve(const Matrix** const J_sp, const Matrix** const B_sp, const Body*** const bodyMapping,
std::map<Body*, uint> bodyNumberMapping, const Vector& b, const Vector& lowLimits,
const Vector& highLimits, Vector& lambda) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda
virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda
};
} // End of the ReactPhysics3D namespace

3
sources/reactphysics3d/mathematics/lcp/LCPSolver.cpp
View File

@ -23,7 +23,8 @@
using namespace reactphysics3d;
// Constructor
LCPSolver::LCPSolver() {
LCPSolver::LCPSolver(uint maxIterations)
: maxIterations(maxIterations) {
}

6
sources/reactphysics3d/mathematics/lcp/LCPSolver.h
View File

@ -60,9 +60,9 @@ class LCPSolver {
public:
LCPSolver(uint maxIterations); // Constructor
virtual ~LCPSolver(); // Destructor
virtual void solve(const Matrix** const J_sp, const Matrix** const B_sp, const Body*** const bodyMapping,
std::map<Body*, uint> bodyNumberMapping, const Vector& b, const Vector& lowLimits,
const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem
virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem
void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector
void setMaxIterations(uint maxIterations); // Set the maximum number of iterations
};