diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index 0c286d74..8af26a44 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/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 -void ConstraintSolver::allocate() { - nbConstraints = 0; + // Initialize the constraint solver before each solving +void ConstraintSolver::initialize() { Constraint* constraint; + nbConstraints = 0; + // For each constraint vector::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 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 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; igetNbAuxConstraints(); } } - -// 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); -} diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index 3675c097..c0ec6986 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/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 activeConstraints; // Current active constraints in the physics world uint nbConstraints; // Total number of constraints (with the auxiliary constraints) - std::set 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 constraintBodies; // Bodies that are implied in some constraint std::map 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 diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index 944e3f5e..0a4a3b19 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/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