Optimization of the memory allocation in the constraint solver

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

sources/reactphysics3d/engine/ConstraintSolver.cpp

@@ -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
+ // Initialize the constraint solver before each solving
-void ConstraintSolver::allocate() {
+void ConstraintSolver::initialize() {
-    nbConstraints = 0;
     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];
+    assert(nbConstraints > 0);
-    J_sp = new Matrix*[nbConstraints];
+    assert(nbBodies > 0);
-    B_sp = new Matrix*[2];
+    
-    B_sp[0] = new Matrix[nbConstraints];
+    // Update the average bodies and constraints capacities
-    B_sp[1] = new Matrix[nbConstraints];
+    if (avBodiesCounter > AV_COUNTER_LIMIT) {
-    for (uint i=0; i<nbConstraints; i++) {
+        avBodiesCounter = 0;
-        bodyMapping[i] = new Body*[2];
+        avBodiesNumber = 0;
-        J_sp[i] = new Matrix[2];
+    }
+    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);
+    // If we need to allocate more memory for the constraints
-    b.changeSize(nbConstraints);
+    if (nbConstraints > constraintsCapacity || constraintsCapacity < AV_PERCENT_TO_FREE * constraintsCapacity) {
-    lambda.changeSize(nbConstraints);
+        freeMemory(false);
-    lambdaInit.changeSize(nbConstraints);
+        constraintsCapacity = nbConstraints;
-    lowerBounds.changeSize(nbConstraints);
+
-    upperBounds.changeSize(nbConstraints);
+        bodyMapping = new Body**[nbConstraints];
-    Minv_sp = new Matrix[nbBodies];
+        J_sp = new Matrix*[nbConstraints];
-    V1 = new Vector[nbBodies];
+        B_sp = new Matrix*[2];
-    Vconstraint = new Vector[nbBodies];
+        B_sp[0] = new Matrix[nbConstraints];
-    Fext = new Vector[nbBodies];
+        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);
-}

sources/reactphysics3d/engine/ConstraintSolver.h

@@ -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,12 +87,14 @@ 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
@@ -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
 

sources/reactphysics3d/engine/PhysicsEngine.cpp

@@ -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