Add ContactManifoldSet class

CMakeLists.txt

@@ -109,6 +109,10 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/CollisionDetection.h"
     "src/collision/CollisionDetection.cpp"
     "src/collision/CollisionShapeInfo.h"
+    "src/collision/ContactManifold.h"
+    "src/collision/ContactManifold.cpp"
+    "src/collision/ContactManifoldSet.h"
+    "src/collision/ContactManifoldSet.cpp"
     "src/constraint/BallAndSocketJoint.h"
     "src/constraint/BallAndSocketJoint.cpp"
     "src/constraint/ContactPoint.h"
@@ -125,8 +129,6 @@ SET (REACTPHYSICS3D_SOURCES
     "src/engine/CollisionWorld.cpp"
     "src/engine/ConstraintSolver.h"
     "src/engine/ConstraintSolver.cpp"
-    "src/engine/ContactManifold.h"
-    "src/engine/ContactManifold.cpp"
     "src/engine/ContactSolver.h"
     "src/engine/ContactSolver.cpp"
     "src/engine/DynamicsWorld.h"

src/body/CollisionBody.cpp

@@ -26,7 +26,7 @@
  // Libraries
 #include "CollisionBody.h"
 #include "engine/CollisionWorld.h"
-#include "engine/ContactManifold.h"
+#include "collision/ContactManifold.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;

src/collision/CollisionDetection.cpp

@@ -431,11 +431,15 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
                                 ContactPoint(*contactInfo);
     assert(contact != NULL);
 
-    // Add the contact to the contact cache of the corresponding overlapping pair
+    // Add the contact to the contact manifold set of the corresponding overlapping pair
     overlappingPair->addContact(contact);
 
     // Add the contact manifold into the list of contact manifolds
     // of the two bodies involved in the contact
+
+    // TODO : It seems that we add the same manifold multiple times to the same body
+    // in case we call createContact() multiple time for different contact points from the
+    // same manifold. Check that it is not the case.
     addContactManifoldToBody(overlappingPair->getContactManifold(),
                              overlappingPair->getShape1()->getBody(),
                              overlappingPair->getShape2()->getBody());

src/collision/ContactManifold.h

@@ -119,6 +119,9 @@ class ContactManifold {
         /// True if the contact manifold has already been added into an island
         bool mIsAlreadyInIsland;
 
+        /// Average contact normal vector of all points in the manifold
+        Vector3 mAverageContactNormal;
+
         /// Reference to the memory allocator
         MemoryAllocator& mMemoryAllocator;
 
@@ -144,6 +147,9 @@ class ContactManifold {
 
         /// Return true if the contact manifold has already been added into an island
         bool isAlreadyInIsland() const;
+
+        /// Return the normalized averaged normal vector
+        Vector3 getAverageContactNormal() const;
         
     public:
 
@@ -306,6 +312,17 @@ inline bool ContactManifold::isAlreadyInIsland() const {
     return mIsAlreadyInIsland;
 }
 
+// Return the normalized averaged normal vector
+inline Vector3 ContactManifold::getAverageContactNormal() const {
+    Vector3 averageNormal;
+
+    for (int i=0; i<mNbContactPoints; i++) {
+        averageNormal += mContactPoints[i]->getNormal();
+    }
+
+    return averageNormal.getUnit();
+}
+
 }
 #endif
 

src/collision/ContactManifoldSet.cpp

@@ -0,0 +1,108 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2015 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ContactManifoldSet.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
+                                       MemoryAllocator& memoryAllocator, int nbMaxManifolds)
+                   : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
+                     mShape2(shape2), mMemoryAllocator(memoryAllocator) {
+
+}
+
+// Destructor
+ContactManifoldSet::~ContactManifoldSet() {
+
+}
+
+// Add a contact point to the manifold set
+void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
+
+    // If there is no contact manifold yet
+    if (mNbManifolds == 0) {
+
+        createManifold();
+        mManifolds[0]->addContactPoint(contact);
+
+        return;
+    }
+
+
+}
+
+// Return the index of the contact manifold with a similar average normal.
+// If no manifold has close enough average normal, it returns -1
+int ContactManifoldSet::selectManifoldWithSimilarNormal(const Vector3& normal) {
+
+    decimal maxDotProduct;
+    int indexManifold = -1;
+    for (int i=0; i<mNbManifolds; i++) {
+        decimal dotProduct = normal.dot(mManifolds[i]->getAverageContactNormal());
+        if (dotProduct > maxDotProduct) {
+            maxDotProduct = dotProduct;
+            indexManifold = i;
+        }
+    }
+
+    return indexManifold;
+}
+
+// Update the contact manifolds
+void ContactManifoldSet::update(const Transform& transform1, const Transform& transform2) {
+
+    for (int i=0; i<mNbManifolds; i++) {
+        mManifolds[i]->update(transform1, transform2);
+    }
+}
+
+// Clear the contact manifold set
+void ContactManifoldSet::clear() {
+
+}
+
+// Create a new contact manifold and add it to the set
+void ContactManifoldSet::createManifold() {
+    assert(mNbManifolds < MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET);
+
+    mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
+                                    ContactManifold(mShape1, mShape2, mMemoryAllocator);
+    mNbManifolds++;
+}
+
+// Remove a contact manifold from the set
+void ContactManifoldSet::removeManifold(int index) {
+
+    assert(index >= 0 && index < mNbManifolds);
+
+    // Delete the new contact
+    mManifolds[index]->~ContactManifold();
+    mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
+
+    mNbManifolds--;
+}

src/collision/ContactManifoldSet.h

@@ -0,0 +1,129 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2015 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
+#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
+
+// Libraries
+#include "ContactManifold.h"
+
+namespace reactphysics3d {
+
+// Constants
+const uint MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3;   // Maximum number of contact manifolds in the set
+
+// Class ContactManifoldSet
+/**
+ * This class represents a set of one or several contact manifolds. Typically a
+ * convex/convex collision will have a set with a single manifold and a convex-concave
+ * collision can have more than one manifolds. Note that a contact manifold can
+ * contains several contact points.
+ */
+class ContactManifoldSet {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Maximum number of contact manifolds in the set
+        int mNbMaxManifolds;
+
+        /// Current number of contact manifolds in the set
+        int mNbManifolds;
+
+        /// Pointer to the first proxy shape of the contact
+        ProxyShape* mShape1;
+
+        /// Pointer to the second proxy shape of the contact
+        ProxyShape* mShape2;
+
+        /// Reference to the memory allocator
+        MemoryAllocator& mMemoryAllocator;
+
+        /// Contact manifolds of the set
+        ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
+
+        // -------------------- Methods -------------------- //
+
+        /// Create a new contact manifold and add it to the set
+        void createManifold();
+
+        /// Remove a contact manifold from the set
+        void removeManifold(int index);
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
+                           MemoryAllocator& memoryAllocator, int nbMaxManifolds);
+
+        /// Destructor
+        ~ContactManifoldSet();
+
+        /// Add a contact point to the manifold set
+        void addContactPoint(ContactPoint* contact);
+
+        /// Update the contact manifolds
+        void update(const Transform& transform1, const Transform& transform2);
+
+        /// Clear the contact manifold set
+        void clear();
+
+        /// Return the number of manifolds in the set
+        int getNbContactManifolds() const;
+
+        /// Return a given contact manifold
+        ContactManifold* getContactManifold(uint index);
+
+        /// Return the total number of contact points in the set of manifolds
+        int getTotalNbContactPoints() const;
+};
+
+// Return the number of manifolds in the set
+inline int ContactManifoldSet::getNbContactManifolds() const {
+    return mNbManifolds;
+}
+
+// Return a given contact manifold
+inline ContactManifold* ContactManifoldSet::getContactManifold(uint index) {
+    assert(index < mNbManifolds);
+    return mManifolds[index];
+}
+
+// Return the total number of contact points in the set of manifolds
+inline int ContactManifoldSet::getTotalNbContactPoints() const {
+    int nbPoints = 0;
+    for (int i=0; i<mNbManifolds; i++) {
+        nbPoints += mManifolds[i]->getNbContactPoints();
+    }
+    return nbPoints;
+}
+
+}
+
+#endif
+

src/configuration.h

@@ -128,6 +128,11 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
 /// followin constant with the linear velocity and the elapsed time between two frames.
 const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
 
+/// This value is used to compare the normal of a contact point and the average
+/// normal of a whole contact manifold to decide if the point should be part
+/// of the manifold or not
+const decimal MANIFOLD_SIMILAR_NORMAL_DOTPRODUCT_THRESHOLD = 0.99f;
+
 }
 
 #endif

src/constraint/ContactPoint.h

@@ -65,7 +65,7 @@ struct ContactPointInfo {
         /// Second proxy shape of the contact
         ProxyShape* shape2;
 
-        /// Normal vector the the collision contact in world space
+        /// Normalized normal vector the the collision contact in world space
         const Vector3 normal;
 
         /// Penetration depth of the contact
@@ -107,7 +107,7 @@ class ContactPoint {
         /// Second rigid body of the contact
         CollisionBody* mBody2;
 
-        /// Normal vector of the contact (From body1 toward body2) in world space
+        /// Normalized normal vector of the contact (From body1 toward body2) in world space
         const Vector3 mNormal;
 
         /// Penetration depth

src/engine/ContactSolver.h

@@ -30,7 +30,7 @@
 #include "constraint/ContactPoint.h"
 #include "configuration.h"
 #include "constraint/Joint.h"
-#include "ContactManifold.h"
+#include "collision/ContactManifold.h"
 #include "Island.h"
 #include "Impulse.h"
 #include <map>

src/engine/Island.h

@@ -30,7 +30,7 @@
 #include "memory/MemoryAllocator.h"
 #include "body/RigidBody.h"
 #include "constraint/Joint.h"
-#include "ContactManifold.h"
+#include "collision/ContactManifold.h"
 
 namespace reactphysics3d {
 

src/engine/OverlappingPair.cpp

@@ -33,7 +33,7 @@ using namespace reactphysics3d;
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& memoryAllocator)
                 : mShape1(shape1), mShape2(shape2),
-                  mContactManifold(shape1, shape2, memoryAllocator),
+                  mContactManifoldSet(shape1, shape2, memoryAllocator),
                   mCachedSeparatingAxis(1.0, 1.0, 1.0) {
     
 }

src/engine/OverlappingPair.h

@@ -27,7 +27,7 @@
 #define	REACTPHYSICS3D_OVERLAPPING_PAIR_H
 
 // Libraries
-#include "ContactManifold.h"
+#include "collision/ContactManifoldSet.h"
 #include "collision/ProxyShape.h"
 #include "collision/shapes/CollisionShape.h"
 
@@ -57,8 +57,8 @@ class OverlappingPair {
         /// Pointer to the second proxy collision shape
         ProxyShape* mShape2;
 
-        /// Persistent contact manifold
-        ContactManifold mContactManifold;
+        /// Set of persistent contact manifolds
+        ContactManifoldSet mContactManifoldSet;
 
         /// Cached previous separating axis
         Vector3 mCachedSeparatingAxis;
@@ -131,13 +131,13 @@ inline ProxyShape* OverlappingPair::getShape2() const {
 
 // Add a contact to the contact manifold
 inline void OverlappingPair::addContact(ContactPoint* contact) {
-    mContactManifold.addContactPoint(contact);
+    mContactManifoldSet.addContactPoint(contact);
 }
 
 // Update the contact manifold
 inline void OverlappingPair::update() {
-    mContactManifold.update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
-                            mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform());
+    mContactManifoldSet.update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
+                               mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform());
 }
 
 // Return the cached separating axis
@@ -153,12 +153,12 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
 
 // Return the number of contact points in the contact manifold
 inline uint OverlappingPair::getNbContactPoints() const {
-    return mContactManifold.getNbContactPoints();
+    return mContactManifoldSet.getTotalNbContactPoints();
 }
 
 // Return the contact manifold
-inline ContactManifold* OverlappingPair::getContactManifold() {
-    return &mContactManifold;
+inline ContactManifold* OverlappingPair::getContactManifoldSet() {
+    return &mContactManifoldSet;
 }
 
 // Return the pair of bodies index
@@ -187,7 +187,7 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
 
 // Clear the contact points of the contact manifold
 inline void OverlappingPair::clearContactPoints() {
-   mContactManifold.clear();
+   mContactManifoldSet.clear();
 }
 
 }