Add the class Contact that contains the contact points between two bodies

src/engine/ConstraintSolver.cpp

@@ -49,45 +49,53 @@ void ConstraintSolver::initialize() {
     nbConstraints = 0;
 
     // TOOD : Use better allocation here
-    mContactConstraints = new ContactConstraint[world->getNbContactConstraints()];
+    mContactConstraints = new ContactConstraint[world->getNbContactManifolds()];
 
     uint nbContactConstraints = 0;
 
-    // For each constraint
+    // For each contact manifold of the world
-    vector<Contact*>::iterator it;
+    vector<ContactManifold>::iterator it;
-    for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) {
+    for (it = world->getContactManifoldsBeginIterator(); it != world->getContactManifoldsEndIterator(); ++it) {
-        Contact* contact = *it;
+        ContactManifold contactManifold = *it;
 
-        // If the constraint is active
+        // For each contact point of the contact manifold
-        if (contact->isActive()) {
+        for (uint c=0; c<contactManifold.nbContacts; c++) {
 
-            RigidBody* body1 = contact->getBody1();
+            // Get a contact point
-            RigidBody* body2 = contact->getBody2();
+            Contact* contact = contactManifold.contacts[c];
 
-            activeConstraints.push_back(contact);
+            // If the constraint is active
+            if (contact->isActive()) {
 
-            // Add the two bodies of the constraint in the constraintBodies list
+                RigidBody* body1 = contact->getBody1();
-            mConstraintBodies.insert(body1);
+                RigidBody* body2 = contact->getBody2();
-            mConstraintBodies.insert(body2);
 
-            // Fill in the body number maping
+                activeConstraints.push_back(contact);
-            mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
-            mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
 
-            // Update the size of the jacobian matrix
+                // Add the two bodies of the constraint in the constraintBodies list
-            nbConstraints += contact->getNbConstraints();
+                mConstraintBodies.insert(body1);
-            
+                mConstraintBodies.insert(body2);
-            ContactConstraint constraint = mContactConstraints[nbContactConstraints];
+
-            constraint.indexBody1 = mMapBodyToIndex[body1];
+                // Fill in the body number maping
-            constraint.indexBody2 = mMapBodyToIndex[body2];
+                mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
-            constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
+                mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
-            constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
+
-            constraint.isBody1Moving = body1->getIsMotionEnabled();
+                // Update the size of the jacobian matrix
-            constraint.isBody2Moving = body2->getIsMotionEnabled();
+                nbConstraints += contact->getNbConstraints();
-            constraint.massInverseBody1 = body1->getMassInverse();
+
-            constraint.massInverseBody2 = body2->getMassInverse();
+                ContactConstraint constraint = mContactConstraints[nbContactConstraints];
+                constraint.indexBody1 = mMapBodyToIndex[body1];
+                constraint.indexBody2 = mMapBodyToIndex[body2];
+                constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
+                constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
+                constraint.isBody1Moving = body1->getIsMotionEnabled();
+                constraint.isBody2Moving = body2->getIsMotionEnabled();
+                constraint.massInverseBody1 = body1->getMassInverse();
+                constraint.massInverseBody2 = body2->getMassInverse();
+
+                nbContactConstraints++;
+            }
 
-            nbContactConstraints++;
         }
     }
 

src/engine/DynamicsWorld.cpp

@@ -50,7 +50,6 @@ DynamicsWorld::~DynamicsWorld() {
 
 // Update the physics simulation
 void DynamicsWorld::update() {
-    bool existCollision = false;
 
     assert(mTimer.getIsRunning());
     
@@ -63,16 +62,16 @@ void DynamicsWorld::update() {
     // While the time accumulator is not empty
     while(mTimer.isPossibleToTakeStep()) {
 
-        existCollision = false;
+        // Remove all contact manifolds
-
+        mContactManifolds.clear();
-        // Remove all contact constraints
-        mContactConstraints.clear();
 		
         // Compute the collision detection
-        if (mCollisionDetection.computeCollisionDetection()) {
+        mCollisionDetection.computeCollisionDetection();
-            existCollision = true;
 
-            // Solve constraints
+        // If there are constraints or contacts
+        if (!mConstraints.empty() || !mContactManifolds.empty()) {
+
+            // Solve the constraints and contacts
             mConstraintSolver.solve(mTimer.getTimeStep());
         }
 
@@ -86,9 +85,7 @@ void DynamicsWorld::update() {
         updateAllBodiesMotion();
 
         // Cleanup of the constraint solver
-        if (existCollision) {
+        mConstraintSolver.cleanup();
-           mConstraintSolver.cleanup();
-        }
     }
 
     // Compute and set the interpolation factor to all the bodies
@@ -302,9 +299,17 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
     // Add the contact to the contact cache of the corresponding overlapping pair
     overlappingPair->addContact(contact);
 
+    // Create a contact manifold with the contact points of the two bodies
+    ContactManifold contactManifold;
+    contactManifold.nbContacts = 0;
+
     // Add all the contacts in the contact cache of the two bodies
     // to the set of constraints in the physics world
     for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
-        mContactConstraints.push_back(overlappingPair->getContact(i));
+        contactManifold.contacts[i] = overlappingPair->getContact(i);
+        contactManifold.nbContacts++;
     }
+
+    // Add the contact manifold to the world
+    mContactManifolds.push_back(contactManifold);
 }

src/engine/DynamicsWorld.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionWorld.h"
+#include "ContactManifold.h"
 #include "../collision/CollisionDetection.h"
 #include "ConstraintSolver.h"
 #include "../body/RigidBody.h"
@@ -63,7 +64,7 @@ class DynamicsWorld : public CollisionWorld {
         std::set<RigidBody*> mRigidBodies;
 
         // All the contact constraints
-        std::vector<Contact*> mContactConstraints;
+        std::vector<ContactManifold> mContactManifolds;
 
         // All the constraints (except contact constraints)
         std::vector<Constraint*> mConstraints;
@@ -171,7 +172,7 @@ public :
         void removeAllConstraints();
 
         // Return the number of contact constraints in the world
-        uint getNbContactConstraints() const;
+        uint getNbContactManifolds() const;
 
         // Return a start iterator on the constraint list
         std::vector<Constraint*>::iterator getConstraintsBeginIterator();
@@ -179,11 +180,11 @@ public :
         // Return a end iterator on the constraint list
         std::vector<Constraint*>::iterator getConstraintsEndIterator();
 
-        // Return a start iterator on the contact constraint list
+        // Return a start iterator on the contact manifolds list
-        std::vector<Contact*>::iterator getContactConstraintsBeginIterator();
+        std::vector<ContactManifold>::iterator getContactManifoldsBeginIterator();
 
-        // Return a end iterator on the contact constraint list
+        // Return a end iterator on the contact manifolds list
-        std::vector<Contact*>::iterator getContactConstraintsEndIterator();
+        std::vector<ContactManifold>::iterator getContactManifoldsEndIterator();
 
         // Return an iterator to the beginning of the rigid bodies of the physics world
         std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@@ -277,9 +278,9 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
     return mRigidBodies.end();
 }
 
-// Return the number of contact constraints in the world
+// Return the number of contact manifolds in the world
-inline uint DynamicsWorld::getNbContactConstraints() const {
+inline uint DynamicsWorld::getNbContactManifolds() const {
-    return mContactConstraints.size();
+    return mContactManifolds.size();
 }
 
 // Return a start iterator on the constraint list
@@ -292,14 +293,14 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
     return mConstraints.end();
 }
 
-// Return a start iterator on the contact constraint list
+// Return a start iterator on the contact manifolds list
-inline std::vector<Contact*>::iterator DynamicsWorld::getContactConstraintsBeginIterator() {
+inline std::vector<ContactManifold>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
-    return mContactConstraints.begin();
+    return mContactManifolds.begin();
 }
 
-// Return a end iterator on the contact constraint list
+// Return a end iterator on the contact manifolds list
-inline std::vector<Contact*>::iterator DynamicsWorld::getContactConstraintsEndIterator() {
+inline std::vector<ContactManifold>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
-    return mContactConstraints.end();
+    return mContactManifolds.end();
 }
 
 }