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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-06-09 21:40:13 +00:00
parent f2beab0fcf
commit ff8c689870
7 changed files with 55 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,8 +25,8 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
:physicsWorld(world), bodyMapping(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
}
@ -37,12 +37,12 @@ ConstraintSolver::~ConstraintSolver() {
// Allocate all the matrices needed to solve the LCP problem
void ConstraintSolver::allocate() {
uint nbConstraints = 0;
nbConstraints = 0;
Constraint* constraint;
// For each constraint
std::vector<Constraint*>::iterator it;
for (it = physicsWorld.getConstraintsBeginIterator(); it <physicsWorld.getConstraintsEndIterator(); it++) {
for (it = physicsWorld->getConstraintsBeginIterator(); it <physicsWorld->getConstraintsEndIterator(); it++) {
constraint = *it;
// Evaluate the constraint
@ -71,17 +71,18 @@ void ConstraintSolver::allocate() {
bodyMapping = new Body**[nbConstraints];
J_sp = new Matrix*[nbConstraints];
B_sp = new Matrix*[2];
B_sp[0] = new Matrix[nbConstraints];
B_sp[1] = new Matrix[nbConstraints];
for (uint i=0; i<nbConstraints; i++) {
bodyMapping[i] = new Body*[2];
J_sp[i] = new Matrix[2];
B_sp[i] = new Matrix[nbConstraints];
}
errorValues = Vector(nbConstraints);
b = Vector(nbConstraints);
lambda = Vector(nbConstraints);
lowerBounds = Vector(nbConstraints);
upperBounds = Vector(nbConstraints);
errorValues.changeSize(nbConstraints);
b.changeSize(nbConstraints);
lambda.changeSize(nbConstraints);
lowerBounds.changeSize(nbConstraints);
upperBounds.changeSize(nbConstraints);
Minv_sp = new Matrix[nbBodies];
V = new Vector[nbBodies];
Fext = new Vector[nbBodies];
@ -92,26 +93,27 @@ void ConstraintSolver::allocate() {
void ConstraintSolver::fillInMatrices() {
// For each active constraint
for (uint c=0; c<activeConstraints.size(); c++) {
for (uint c=0, nbAuxConstraints=0; c<activeConstraints.size(); c += 1 + nbAuxConstraints) {
Constraint* constraint = activeConstraints.at(c);
// Fill in the J_sp matrix
J_sp[c][0].changeSize(1, 6);
J_sp[c][1].changeSize(1, 6);
J_sp[c][0] = constraint->getBody1Jacobian();
J_sp[c][1] = constraint->getBody2Jacobian();
// Fill in the body mapping matrix
bodyMapping[c][0] = constraint->getBody1();
bodyMapping[c][1] = constraint->getBody2();
// Fill in the limit vectors for the constraint
lowerBounds.fillInSubVector(c, constraint->getLowerBound());
upperBounds.fillInSubVector(c, constraint->getUpperBound());
lowerBounds.setValue(c, constraint->getLowerBound());
upperBounds.setValue(c, constraint->getUpperBound());
// Fill in the error vector
errorValues.fillInSubVector(c, constraint->getErrorValue());
errorValues.setValue(c, constraint->getErrorValue());
uint nbAuxConstraints = constraint->getNbAuxConstraints();
nbAuxConstraints = constraint->getNbAuxConstraints();
// If the current constraint has auxiliary constraints
if (nbAuxConstraints > 0) {
@ -119,6 +121,8 @@ void ConstraintSolver::fillInMatrices() {
// For each auxiliary constraints
for (uint i=1; i<=nbAuxConstraints; i++) {
// Fill in the J_sp matrix
J_sp[c+i][0].changeSize(1, 6);
J_sp[c+i][1].changeSize(1, 6);
J_sp[c+i][0] = constraint->getAuxJacobian().getSubMatrix(i-1, 0, 1, 6);
J_sp[c+i][1] = constraint->getAuxJacobian().getSubMatrix(i-1, 6, 1, 6);
@ -149,11 +153,13 @@ void ConstraintSolver::fillInMatrices() {
// Compute the vector with velocities values
v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity());
v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity());
V[bodyNumber].changeSize(6);
V[bodyNumber] = v;
// Compute the vector with forces and torques values
f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce());
f.fillInSubVector(3, rigidBody->getCurrentBodyState().getExternalTorque());
Fext[bodyNumber].changeSize(6);
Fext[bodyNumber] = f;
// Compute the inverse sparse mass matrix
@ -161,6 +167,7 @@ void ConstraintSolver::fillInMatrices() {
mInv.initWithValue(0.0);
mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
mInv.fillInSubMatrix(3, 3, rigidBody->getCurrentBodyState().getInertiaTensorInverse());
Minv_sp[bodyNumber].changeSize(6, 6);
Minv_sp[bodyNumber] = mInv;
}
}
@ -170,13 +177,17 @@ void ConstraintSolver::freeMemory() {
activeConstraints.clear();
bodyNumberMapping.clear();
constraintBodies.clear();
// Free the bodyMaping array
for (uint i=0; i<nbBodies; i++) {
for (uint i=0; i<nbConstraints; i++) {
delete[] bodyMapping[i];
delete[] J_sp[i];
}
delete[] bodyMapping;
delete[] J_sp;
delete[] B_sp[0];
delete[] B_sp[1];
delete[] B_sp;
delete[] Minv_sp;
delete[] V;
@ -190,7 +201,7 @@ void ConstraintSolver::computeVectorB(double dt) {
b = errorValues * oneOverDT;
for (uint c = 0; c<activeConstraints.size(); c++) {
for (uint c = 0; c<nbConstraints; c++) {
// Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
@ -208,9 +219,11 @@ void ConstraintSolver::computeMatrixB_sp() {
uint indexBody1, indexBody2;
// For each constraint
for (uint c = 0; c<activeConstraints.size(); c++) {
for (uint c = 0; c<nbConstraints; c++) {
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
B_sp[0][c].changeSize(6,1);
B_sp[1][c].changeSize(6,1);
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
}
@ -226,7 +239,7 @@ void ConstraintSolver::computeVectorV(double dt) {
uint indexBody1, indexBody2;
// Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<activeConstraints.size(); i++) {
for (uint i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
V[indexBody1] = V[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
@ -254,7 +267,9 @@ void ConstraintSolver::solve(double dt) {
computeMatrixB_sp();
// Solve the LCP problem (computation of lambda)
lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
Vector lambdaInit(nbConstraints);
lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Compute the vector V2
computeVectorV(dt);

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

@ -41,9 +41,10 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol
*/
class ConstraintSolver {
protected:
PhysicsWorld& physicsWorld; // Reference to the physics world
PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
uint nbBodies; // Current number of bodies in the physics world
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
@ -76,7 +77,7 @@ class ConstraintSolver {
void computeVectorV(double dt); // Compute the vector V2
public:
ConstraintSolver(PhysicsWorld& world); // Constructor
ConstraintSolver(PhysicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem
};

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

@ -26,7 +26,7 @@ using namespace reactphysics3d;
// Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument)
: world(world), timer(Time(0.0), timeStep), collisionDetection(world) {
: world(world), timer(Time(0.0), timeStep), collisionDetection(world), constraintSolver(world) {
// Check if the pointer to the world is not NULL
if (world == 0) {
// Throw an exception
@ -89,13 +89,18 @@ void PhysicsEngine::updateCollision() {
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
// TODO : Delete this ----------------------------------------------------------
/*
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);
rigidBody2->setIsMotionEnabled(false);
}
*/
// -----------------------------------------------------------------------------
// Solve constraints
constraintSolver.solve(timer.getTimeStep().getValue());
}
// For each body in the dynamic world

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

@ -24,6 +24,7 @@
#include "PhysicsWorld.h"
#include "../integration/IntegrationAlgorithm.h"
#include "../collision/CollisionDetection.h"
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
@ -42,6 +43,7 @@ class PhysicsEngine {
Timer timer; // Timer of the physics engine
IntegrationAlgorithm* integrationAlgorithm; // Integration algorithm used to solve differential equations of movement
CollisionDetection collisionDetection; // Collision detection
ConstraintSolver constraintSolver; // Constraint solver
void updateBodyState(RigidBody* const rigidBody, const Time& timeStep); // Update the state of a rigid body

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

@ -304,7 +304,7 @@ double Matrix::getTrace() const throw(MathematicsException) {
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) {
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");
}
@ -475,10 +475,10 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException
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++) {
Matrix result(nbRow, 1);
for (int i=0; i<nbRow; i++) {
double sum = 0.0;
for (int j=0; j<vector.getNbComponent(); j++) {
for (int j=0; j<nbColumn; j++) {
sum += array[i][j] * vector.getValue(j);
}
result.array[i][0] = sum;

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

@ -39,6 +39,9 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
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 {
int size1 = lambda.getNbComponent();
int size2 = lambdaInit.getNbComponent();
lambda = lambdaInit;
double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
uint indexBody1, indexBody2;

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

@ -69,6 +69,7 @@ class LCPSolver {
// Set the initial lambda vector
inline void LCPSolver::setLambdaInit(const Vector& lambdaInit) {
this->lambdaInit.changeSize(lambdaInit.getNbComponent());
this->lambdaInit = lambdaInit;
}