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

Optimization of the memory allocation in the constraint solver

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@381 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-08-27 09:21:33 +00:00
parent 592cfa4ad1
commit 898e9ba6f3
3 changed files with 160 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,7 +27,10 @@ using namespace std;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
:physicsWorld(world), bodyMapping(0), nbConstraints(0), constraintsCapacity(0),
bodiesCapacity(0), avConstraintsCapacity(0), avBodiesCapacity(0), avBodiesNumber(0),
avConstraintsNumber(0), avBodiesCounter(0), avConstraintsCounter(0),
lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
}
@ -36,11 +39,12 @@ ConstraintSolver::~ConstraintSolver() {
}
// Allocate all the matrices needed to solve the LCP problem
void ConstraintSolver::allocate() {
nbConstraints = 0;
// Initialize the constraint solver before each solving
void ConstraintSolver::initialize() {
Constraint* constraint;
nbConstraints = 0;
// For each constraint
vector<Constraint*>::iterator it;
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
@ -66,31 +70,109 @@ void ConstraintSolver::allocate() {
}
}
assert(nbConstraints > 0);
// Compute the number of bodies that are part of some active constraint
nbBodies = bodyNumberMapping.size();
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];
assert(nbConstraints > 0);
assert(nbBodies > 0);
// Update the average bodies and constraints capacities
if (avBodiesCounter > AV_COUNTER_LIMIT) {
avBodiesCounter = 0;
avBodiesNumber = 0;
}
if (avConstraintsCounter > AV_COUNTER_LIMIT) {
avConstraintsCounter = 0;
avConstraintsNumber = 0;
}
avBodiesCounter++;
avConstraintsCounter++;
avBodiesNumber += nbBodies;
avConstraintsNumber += nbConstraints;
avBodiesCapacity += (avBodiesNumber / avBodiesCounter);
avConstraintsCapacity += (avConstraintsNumber / avConstraintsCounter);
// Allocate the memory needed for the constraint solver
allocate();
}
// Allocate all the memory needed to solve the LCP problem
// The goal of this method is to avoid to free and allocate the memory
// each time the constraint solver is called but only if the we effectively
// need more memory. Therefore if for instance the number of constraints to
// be solved is smaller than the constraints capacity, we don't free and reallocate
// memory because we don't need to. The problem now is that the constraints capacity
// can grow indefinitely. Therefore we use a way to free and reallocate the memory
// if the average number of constraints currently solved is far less than the current
// constraints capacity
void ConstraintSolver::allocate() {
// If we need to allocate more memory for the bodies
if (nbBodies > bodiesCapacity || avBodiesCapacity < AV_PERCENT_TO_FREE * bodiesCapacity) {
freeMemory(true);
bodiesCapacity = nbBodies;
Minv_sp = new Matrix[nbBodies];
V1 = new Vector[nbBodies];
Vconstraint = new Vector[nbBodies];
Fext = new Vector[nbBodies];
avBodiesNumber = 0;
avBodiesCounter = 0;
}
errorValues.changeSize(nbConstraints);
b.changeSize(nbConstraints);
lambda.changeSize(nbConstraints);
lambdaInit.changeSize(nbConstraints);
lowerBounds.changeSize(nbConstraints);
upperBounds.changeSize(nbConstraints);
Minv_sp = new Matrix[nbBodies];
V1 = new Vector[nbBodies];
Vconstraint = new Vector[nbBodies];
Fext = new Vector[nbBodies];
// If we need to allocate more memory for the constraints
if (nbConstraints > constraintsCapacity || constraintsCapacity < AV_PERCENT_TO_FREE * constraintsCapacity) {
freeMemory(false);
constraintsCapacity = nbConstraints;
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];
}
errorValues.changeSize(nbConstraints);
b.changeSize(nbConstraints);
lambda.changeSize(nbConstraints);
lambdaInit.changeSize(nbConstraints);
lowerBounds.changeSize(nbConstraints);
upperBounds.changeSize(nbConstraints);
avConstraintsNumber = 0;
avConstraintsCounter = 0;
}
}
// Free the memory that was allocated in the allocate() method
// If the argument is true the method will free the memory
// associated to the bodies. In the other case, it will free
// the memory associated with the constraints
void ConstraintSolver::freeMemory(bool freeBodiesMemory) {
// If we need to free the bodies memory
if (freeBodiesMemory && bodiesCapacity > 0) {
delete[] Minv_sp;
delete[] V1;
delete[] Vconstraint;
delete[] Fext;
}
else if (constraintsCapacity > 0) { // If we need to free the constraints memory
// Free the bodyMaping array
for (uint i=0; i<constraintsCapacity; i++) {
delete[] bodyMapping[i];
delete[] J_sp[i];
}
delete[] bodyMapping;
delete[] J_sp;
delete[] B_sp[0];
delete[] B_sp[1];
delete[] B_sp;
}
}
// Fill in all the matrices needed to solve the LCP problem
@ -211,30 +293,6 @@ void ConstraintSolver::fillInMatrices() {
}
}
// Free the memory that was allocated in the allocate() method
void ConstraintSolver::freeMemory() {
activeConstraints.clear();
bodyNumberMapping.clear();
constraintBodies.clear();
// Free the bodyMaping array
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[] V1;
delete[] Vconstraint;
delete[] Fext;
}
// Compute the vector b
void ConstraintSolver::computeVectorB(double dt) {
uint indexBody1, indexBody2;
@ -310,28 +368,3 @@ void ConstraintSolver::updateContactCache() {
noConstraint += 1 + activeConstraints.at(c)->getNbAuxConstraints();
}
}
// Solve the current LCP problem
void ConstraintSolver::solve(double dt) {
// Allocate memory for the matrices
allocate();
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices();
// Compute the vector b
computeVectorB(dt);
// Compute the matrix B
computeMatrixB_sp();
// Solve the LCP problem (computation of lambda)
lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Update the contact chaching informations
updateContactCache();
// Compute the vector Vconstraint
computeVectorVconstraint(dt);
}

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

@ -34,6 +34,11 @@ namespace reactphysics3d {
// Constants
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter
const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is
// less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This
// is used to avoid to keep to much memory for a long time if the system doesn't
// need that memory. This value is between 0.0 and 1.0
/* -------------------------------------------------------------------
Class ConstrainSolver :
@ -48,8 +53,16 @@ class ConstraintSolver {
ContactCache contactCache; // Contact cache
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
uint nbBodies; // Current number of bodies in the physics world
uint constraintsCapacity; // Number of constraints that are currently allocated in memory in the solver
uint bodiesCapacity; // Number of bodies that are currently allocated in memory in the solver
uint avConstraintsCapacity; // Average constraint capacity
uint avBodiesCapacity; // Average bodies capacity
uint avBodiesNumber; // Total bodies number for average computation
uint avConstraintsNumber; // Total constraints number for average computation
uint avBodiesCounter; // Counter used to compute the average
uint avConstraintsCounter;
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
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
@ -74,13 +87,15 @@ class ConstraintSolver {
Vector* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
Vector* Fext; // Array that contains for each body the vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque.
void allocate(); // Allocate all the matrices needed to solve the LCP problem
void initialize(); // Initialize the constraint solver before each solving
void allocate(); // Allocate all the memory needed to solve the LCP problem
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp
void computeVectorVconstraint(double dt); // Compute the vector V2
void updateContactCache(); // Clear and Fill in the contact cache with the new lambda values
void freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver
public:
ConstraintSolver(PhysicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
@ -88,7 +103,7 @@ class ConstraintSolver {
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3D getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3D getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
void freeMemory(); // Free the memory that was allocated in the allocate() method
void cleanup();
};
// Return true if the body is in at least one constraint
@ -114,6 +129,37 @@ inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
}
// Solve the current LCP problem
inline void ConstraintSolver::solve(double dt) {
// Allocate memory for the matrices
initialize();
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices();
// Compute the vector b
computeVectorB(dt);
// Compute the matrix B
computeMatrixB_sp();
// Solve the LCP problem (computation of lambda)
lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Update the contact chaching informations
updateContactCache();
// Compute the vector Vconstraint
computeVectorVconstraint(dt);
}
// Cleanup of the constraint solver
inline void ConstraintSolver::cleanup() {
bodyNumberMapping.clear();
constraintBodies.clear();
activeConstraints.clear();
}
} // End of ReactPhysics3D namespace

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

@ -47,7 +47,7 @@ PhysicsEngine::~PhysicsEngine() {
// Update the physics simulation
void PhysicsEngine::update() throw (logic_error) {
bool existCollision = false;
bool existCollision = false; // TODO : Delete this if we don't need it
// Check that the timer is running
if (timer.getIsRunning()) {
@ -61,6 +61,7 @@ void PhysicsEngine::update() throw (logic_error) {
// While the time accumulator is not empty
while(timer.isPossibleToTakeStep()) {
existCollision = false;
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
existCollision = true;
@ -75,9 +76,9 @@ void PhysicsEngine::update() throw (logic_error) {
// Update the position and orientation of each body
updateAllBodiesMotion();
// Free the allocated memory of the constraint solver
// Cleanup of the constraint solver
if (existCollision) {
constraintSolver.freeMemory();
constraintSolver.cleanup();
}
// Clear the added and removed bodies from last update() method call