Modify the contact solver so that its main loop is outside the solver

examples/fallingcubes/FallingCubes.cpp

@@ -66,12 +66,11 @@ int main(int argc, char** argv) {
     glutMouseFunc(mouseButton);
     glutMotionFunc(mouseMotion);
     glutKeyboardFunc(keyboard);
+    glutCloseFunc(finish);
 
     // Glut main looop
     glutMainLoop();
 
-    finish();
-
     return 0;
 }
 
@@ -115,8 +114,7 @@ void keyboard(unsigned char key, int x, int y) {
 
         // Escape key
         case 27:
-            finish();
-            exit(0);
+            glutLeaveMainLoop();
             break;
 
         // Space bar

src/engine/CollisionWorld.cpp

@@ -154,6 +154,7 @@ CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& colli
     // A similar collision shape does not already exist in the world, so we create a
     // new one and add it to the world
     void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes());
+    size_t test = collisionShape.getSizeInBytes();
     CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory);
     mCollisionShapes.push_back(newCollisionShape);
 
@@ -181,11 +182,14 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
         // Remove the shape from the set of shapes in the world
         mCollisionShapes.remove(collisionShape);
 
+        // Compute the size (in bytes) of the collision shape
+        size_t nbBytesShape = collisionShape->getSizeInBytes();
+
         // Call the destructor of the collision shape
         collisionShape->CollisionShape::~CollisionShape();
 
         // Deallocate the memory used by the collision shape
-        mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
+        mMemoryAllocator.release(collisionShape, nbBytesShape);
     }
 }
 

src/engine/ConstraintSolver.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "ConstraintSolver.h"
+#include "Profiler.h"
 
 using namespace reactphysics3d;
 
@@ -43,3 +44,19 @@ ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
 ConstraintSolver::~ConstraintSolver() {
 
 }
+
+// Initialize the constraint solver
+void ConstraintSolver::initialize(decimal dt) {
+
+    PROFILE("ConstraintSolver::initialize()");
+
+    // Set the current time step
+    mTimeStep = dt;
+}
+
+// Solve the constraints
+void ConstraintSolver::solve() {
+
+    PROFILE("ConstraintSolver::solve()");
+
+}

src/engine/ConstraintSolver.h

@@ -142,6 +142,12 @@ class ConstraintSolver {
 
         /// Destructor
         ~ConstraintSolver();
+
+        /// Initialize the constraint solver
+        void initialize(decimal dt);
+
+        /// Solve the constraints
+        void solve();
 };
 
 }

src/engine/ContactSolver.cpp

@@ -44,7 +44,6 @@ ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
                              std::vector<Vector3>& constrainedAngularVelocities,
                              const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
               :mContactManifolds(contactManifolds),
-               mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
                mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
                mContactConstraints(NULL),
                mConstrainedLinearVelocities(constrainedLinearVelocities),
@@ -61,7 +60,12 @@ ContactSolver::~ContactSolver() {
 }
 
 // Initialize the constraint solver
-void ContactSolver::initialize() {
+void ContactSolver::initialize(decimal dt) {
+
+    PROFILE("ContactSolver::initialize()");
+
+    // Set the current time step
+    mTimeStep = dt;
 
     // TODO : Use better memory allocation here
     mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
@@ -187,6 +191,9 @@ void ContactSolver::initialize() {
 
     // Initialize the split impulse velocities
     initializeSplitImpulseVelocities();
+
+    // Fill-in all the matrices needed to solve the LCP problem
+    initializeContactConstraints();
 }
 
 // Initialize the split impulse velocities
@@ -380,6 +387,9 @@ void ContactSolver::initializeContactConstraints() {
 /// the solution of the linear system
 void ContactSolver::warmStart() {
 
+    // Check that warm starting is active
+    if (!mIsWarmStartingActive) return;
+
     // For each constraint
     for (uint c=0; c<mNbContactManifolds; c++) {
 
@@ -519,15 +529,13 @@ void ContactSolver::warmStart() {
     }
 }
 
-// Solve the contact constraints by applying sequential impulses
-void ContactSolver::solveContactConstraints() {
+// Solve the contacts
+void ContactSolver::solve() {
+
+    PROFILE("ContactSolver::solve()");
 
     decimal deltaLambda;
     decimal lambdaTemp;
-    uint iter;
-
-    // For each iteration of the contact solver
-    for(iter=0; iter<mNbIterations; iter++) {
 
     // For each contact manifold
     for (uint c=0; c<mNbContactManifolds; c++) {
@@ -745,33 +753,6 @@ void ContactSolver::solveContactConstraints() {
         }
     }
 }
-}
-
-// Solve the constraints
-void ContactSolver::solve(decimal timeStep) {
-
-    PROFILE("ContactSolver::solve()");
-
-    // Set the current time step
-    mTimeStep = timeStep;
-
-    // Initialize the solver
-    initialize();
-
-    // Fill-in all the matrices needed to solve the LCP problem
-    initializeContactConstraints();
-
-    // Warm start the solver
-    if (mIsWarmStartingActive) {
-        warmStart();
-    }
-
-    // Solve the contact constraints
-    solveContactConstraints();
-
-    // Cache the lambda values in order to use them in the next step
-    storeImpulses();
-}
 
 // Store the computed impulses to use them to
 // warm start the solver at the next iteration

src/engine/ContactSolver.h

@@ -345,9 +345,6 @@ class ContactSolver {
         /// Reference to all the contact manifold of the world
         std::vector<ContactManifold*>& mContactManifolds;
 
-        /// Number of iterations of the contact solver
-        uint mNbIterations;
-
         /// Split linear velocities for the position contact solver (split impulse)
         Vector3* mSplitLinearVelocities;
 
@@ -389,25 +386,12 @@ class ContactSolver {
 
         // -------------------- Methods -------------------- //
 
-        /// Initialize the constraint solver
-        void initialize();
-
         /// Initialize the split impulse velocities
         void initializeSplitImpulseVelocities();
 
         /// Initialize the contact constraints before solving the system
         void initializeContactConstraints();
 
-        /// Store the computed impulses to use them to
-        /// warm start the solver at the next iteration
-        void storeImpulses();
-
-        /// Warm start the solver.
-        void warmStart();
-
-        /// Solve the contact constraints by applying sequential impulses
-        void solveContactConstraints();
-
         /// Apply an impulse to the two bodies of a constraint
         void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
 
@@ -460,8 +444,18 @@ class ContactSolver {
         /// Destructor
         virtual ~ContactSolver();
 
-        /// Solve the constraints
-        void solve(decimal timeStep);
+        /// Initialize the constraint solver
+        void initialize(decimal dt);
+
+        /// Warm start the solver.
+        void warmStart();
+
+        /// Store the computed impulses to use them to
+        /// warm start the solver at the next iteration
+        void storeImpulses();
+
+        /// Solve the contacts
+        void solve();
 
         /// Return true if the body is in at least one constraint
         bool isConstrainedBody(RigidBody* body) const;
@@ -481,9 +475,6 @@ class ContactSolver {
         /// Clean up the constraint solver
         void cleanup();
 
-        /// Set the number of iterations of the constraint solver
-        void setNbIterationsSolver(uint nbIterations);
-
         /// Activate or Deactivate the split impulses for contacts
         void setIsSplitImpulseActive(bool isActive);
 
@@ -511,11 +502,6 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
     return mSplitAngularVelocities[indexBody];
 }
 
-// Set the number of iterations of the constraint solver
-inline void ContactSolver::setNbIterationsSolver(uint nbIterations) {
-    mNbIterations = nbIterations;
-}
-
 // Activate or Deactivate the split impulses for contacts
 inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
     mIsSplitImpulseActive = isActive;

src/engine/DynamicsWorld.cpp

@@ -38,6 +38,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
                                mMapBodyToConstrainedVelocityIndex),
                 mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
                                   mMapBodyToConstrainedVelocityIndex),
+                mNbSolverIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
                 mIsDeactivationActive(DEACTIVATION_ENABLED) {
 
 }
@@ -94,15 +95,11 @@ void DynamicsWorld::update() {
         // Compute the collision detection
         mCollisionDetection.computeCollisionDetection();
 
-        // Initialize the constrained velocities
-        initConstrainedVelocitiesArray();
+        // Integrate the velocities
+        integrateRigidBodiesVelocities();
 
-        // If there are contacts
-        if (!mContactManifolds.empty()) {
-
-            // Solve the contacts
-            mContactSolver.solve(static_cast<decimal>(mTimer.getTimeStep()));
-        }
+        // Solve the contacts and constraints
+        solveContactsAndConstraints();
 
         // Update the timer
         mTimer.nextStep();
@@ -110,8 +107,8 @@ void DynamicsWorld::update() {
         // Reset the movement boolean variable of each body to false
         resetBodiesMovementVariable();
 
-        // Update the position and orientation of each body
-        updateRigidBodiesPositionAndOrientation();
+        // Integrate the position and orientation of each body
+        integrateRigidBodiesPositions();
 
         // Cleanup of the contact solver
         mContactSolver.cleanup();
@@ -124,8 +121,8 @@ void DynamicsWorld::update() {
     setInterpolationFactorToAllBodies();
 }
 
-// Update the position and orientation of the rigid bodies
-void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
+// Integrate position and orientation of the rigid bodies
+void DynamicsWorld::integrateRigidBodiesPositions() {
 
     PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()");
 
@@ -200,8 +197,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
     }
 }
 
-// Initialize the constrained velocities array at each step
-void DynamicsWorld::initConstrainedVelocitiesArray() {
+// Integrate the velocities of rigid bodies
+void DynamicsWorld::integrateRigidBodiesVelocities() {
 
     // TODO : Use better memory allocation here
     mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
@@ -228,6 +225,50 @@ void DynamicsWorld::initConstrainedVelocitiesArray() {
     assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
 }
 
+// Solve the contacts and constraints
+void DynamicsWorld::solveContactsAndConstraints() {
+
+    PROFILE("DynamicsWorld::solveContactsAndConstraints()");
+
+    // Get the current time step
+    decimal dt = static_cast<decimal>(mTimer.getTimeStep());
+
+    // Check if there are contacts and constraints to solve
+    bool isConstraintsToSolve = !mJoints.empty();
+    bool isContactsToSolve = !mContactManifolds.empty();
+    if (!isConstraintsToSolve && !isContactsToSolve) return;
+
+    // If there are contacts
+    if (isContactsToSolve) {
+
+        // Initialize the solver
+        mContactSolver.initialize(dt);
+
+        // Warm start the contact solver
+        mContactSolver.warmStart();
+    }
+
+    // If there are constraints
+    if (isConstraintsToSolve) {
+
+        // Initialize the constraint solver
+        mConstraintSolver.initialize(dt);
+    }
+
+    // For each iteration of the solver
+    for (uint i=0; i<mNbSolverIterations; i++) {
+
+        // Solve the constraints
+        if (isConstraintsToSolve) mConstraintSolver.solve();
+
+        // Solve the contacts
+        if (isContactsToSolve) mContactSolver.solve();
+    }
+
+    // Cache the lambda values in order to use them in the next step
+    if (isContactsToSolve) mContactSolver.storeImpulses();
+}
+
 // Cleanup the constrained velocities array at each step
 void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
 
@@ -235,9 +276,6 @@ void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
     mConstrainedLinearVelocities.clear();
     mConstrainedAngularVelocities.clear();
 
-    // Clear the constrained bodies
-    mConstrainedBodies.clear();
-
     // Clear the rigid body to velocities array index mapping
     mMapBodyToConstrainedVelocityIndex.clear();
 }

src/engine/DynamicsWorld.h

@@ -59,6 +59,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Constraint solver
         ConstraintSolver mConstraintSolver;
 
+        /// Number of solver iterations for the Sequential Impulses technique
+        uint mNbSolverIterations;
+
         /// True if the deactivation (sleeping) of inactive bodies is enabled
         bool mIsDeactivationActive;
 
@@ -71,9 +74,6 @@ class DynamicsWorld : public CollisionWorld {
         /// All the joints of the world
         std::set<Constraint*> mJoints;
 
-        /// All the bodies that are part of contacts or constraints
-        std::set<RigidBody*> mConstrainedBodies;
-
         /// Gravity vector of the world
         Vector3 mGravity;
 
@@ -99,8 +99,8 @@ class DynamicsWorld : public CollisionWorld {
         /// Private assignment operator
         DynamicsWorld& operator=(const DynamicsWorld& world);
 
-        /// Compute the motion of all bodies and update their positions and orientations
-        void updateRigidBodiesPositionAndOrientation();
+        /// Integrate the positions and orientations of rigid bodies
+        void integrateRigidBodiesPositions();
 
         /// Update the position and orientation of a body
         void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
@@ -109,8 +109,11 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
-        /// Initialize the constrained velocities array at each step
-        void initConstrainedVelocitiesArray();
+        /// Integrate the velocities of rigid bodies
+        void integrateRigidBodiesVelocities();
+
+        /// Solve the contacts and constraints
+        void solveContactsAndConstraints();
 
         /// Cleanup the constrained velocities array at each step
         void cleanupConstrainedVelocitiesArray();
@@ -212,7 +215,7 @@ inline void DynamicsWorld::stop() {
 
 // Set the number of iterations of the constraint solver
 inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
-    mContactSolver.setNbIterationsSolver(nbIterations);
+    mNbSolverIterations = nbIterations;
 }
 
 // Activate or Deactivate the split impulses for contacts