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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-04-07 14:40:46 +00:00
parent d15751c1de
commit 6ed69219c7
2 changed files with 29 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,25 +37,30 @@ ConstraintSolver::~ConstraintSolver() {
// Allocate all the matrices needed to solve the LCP problem // Allocate all the matrices needed to solve the LCP problem
void ConstraintSolver::allocate() { void ConstraintSolver::allocate() {
unsigned int nbConstraints = 0; unsigned int nbConstraints = 0;
nbBodies = physicsWorld.getBodies().size();
// TODO : Now we keep every bodies of the physics world in the "bodies" std:vector of the constraint solver.
// but maybe we could only keep track of the body that are part of some constraints.
// For each constraint // For each constraint
for (unsigned int i=0; i<physicsWorld.getConstraints().size(); ++i) { for (unsigned int c=0; c<physicsWorld.getConstraints().size(); c++) {
Constraint* constraint = physicsWorld.getConstraints().at(c);
// Evaluate the constraint // Evaluate the constraint
physicsWorld.getConstraints().at(i)->evaluate(); constraint->evaluate();
// If the constraint is active // If the constraint is active
if (physicsWorld.getConstraints().at(i)->isActive()) { if (constraint->isActive()) {
activeConstraints.push_back(physicsWorld.getConstraints().at(i)); activeConstraints.push_back(constraint);
// Fill in the body number maping
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
// Update the size of the jacobian matrix // Update the size of the jacobian matrix
nbConstraints += (1 + physicsWorld.getConstraints().at(i)->getNbAuxConstraints()); nbConstraints += (1 + constraint->getNbAuxConstraints());
} }
} }
// Compute the number of bodies that are part of some active constraint
nbBodies = bodyNumberMapping.size();
bodyMapping = new Body**[nbConstraints]; bodyMapping = new Body**[nbConstraints];
for (unsigned int i=0; i<nbConstraints; i++) { for (unsigned int i=0; i<nbConstraints; i++) {
bodyMapping[i] = new Body*[2]; bodyMapping[i] = new Body*[2];
@ -117,8 +122,9 @@ void ConstraintSolver::fillInMatrices() {
} }
// For each current body of the physics world // For each current body of the physics world
for (unsigned int b=0; b<physicsWorld->getBodies().size(); b++) { for (unsigned int b=0; b<nbBodies; b++) {
i = 6*b; // Compute the vector with velocities values
V.fillInSubVector(b*3, )
Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse()); Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse());
u.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getLinearVelocity()); u.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getLinearVelocity());
@ -133,6 +139,10 @@ void ConstraintSolver::fillInMatrices() {
// Free the memory that was allocated in the allocate() method // Free the memory that was allocated in the allocate() method
void ConstraintSolver::freeMemory() { void ConstraintSolver::freeMemory() {
activeConstraints.clear();
bodyNumberMapping.clear();
// Free the bodyMaping array // Free the bodyMaping array
for (unsigned int i=0; i<nbBodies; i++) { for (unsigned int i=0; i<nbBodies; i++) {
delete[] bodyMapping[i]; delete[] bodyMapping[i];
@ -141,15 +151,9 @@ void ConstraintSolver::freeMemory() {
} }
// Solve the current LCP problem // Solve the current LCP problem
void ConstraintSolver::solve(std::vector<Constraint*>& constraints, std::vector<Body*>* bodies, double dt) { void ConstraintSolver::solve(double dt) {
// Update the current set of bodies in the physics world
this->bodies = bodies;
// Delete all the current actice constraints
activeConstraints.clear();
// Allocate memory for the matrices // Allocate memory for the matrices
allocate(constraints, dt); allocate();
// Fill-in all the matrices needed to solve the LCP problem // Fill-in all the matrices needed to solve the LCP problem
fillInMatrices(); fillInMatrices();
@ -158,4 +162,6 @@ void ConstraintSolver::solve(std::vector<Constraint*>& constraints, std::vector<
lcpSolver.solve(A, b, lowLimits, highLimits, lambda); lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
// TODO : Implement this method ... // TODO : Implement this method ...
freeMemory();
} }

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

@ -65,7 +65,7 @@ class ConstraintSolver {
public: public:
ConstraintSolver(); // Constructor ConstraintSolver(); // Constructor
virtual ~ConstraintSolver(); // Destructor virtual ~ConstraintSolver(); // Destructor
void solve(std::vector<Constraint*>& constraints, std::vector<Body*>* bodies, double dt); // Solve the current LCP problem void solve(double dt); // Solve the current LCP problem
}; };
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace