Merge branch 'opti_collision' into optimization

CMakeLists.txt

@@ -80,7 +80,6 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/narrowphase/GJK/GJKAlgorithm.h"
     "src/collision/narrowphase/GJK/GJKAlgorithm.cpp"
     "src/collision/narrowphase/NarrowPhaseAlgorithm.h"
-    "src/collision/narrowphase/NarrowPhaseAlgorithm.cpp"
     "src/collision/narrowphase/SphereVsSphereAlgorithm.h"
     "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
     "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
@@ -121,7 +120,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/TriangleMesh.cpp"
     "src/collision/CollisionDetection.h"
     "src/collision/CollisionDetection.cpp"
-    "src/collision/CollisionShapeInfo.h"
+    "src/collision/NarrowPhaseInfo.h"
     "src/collision/ContactManifold.h"
     "src/collision/ContactManifold.cpp"
     "src/collision/ContactManifoldSet.h"
@@ -173,11 +172,13 @@ SET (REACTPHYSICS3D_SOURCES
     "src/mathematics/Vector3.h"
     "src/mathematics/Ray.h"
     "src/mathematics/Vector3.cpp"
+    "src/memory/Allocator.h"
     "src/memory/PoolAllocator.h"
     "src/memory/PoolAllocator.cpp"
     "src/memory/SingleFrameAllocator.h"
     "src/memory/SingleFrameAllocator.cpp"
-    "src/memory/Stack.h"
+    "src/containers/Stack.h"
+    "src/containers/LinkedList.h"
 )
 
 # Create the library

src/body/CollisionBody.h

@@ -153,6 +153,9 @@ class CollisionBody : public Body {
         /// Raycast method with feedback information
         bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
 
+        /// Test if the collision body overlaps with a given AABB
+        bool testAABBOverlap(const AABB& worldAABB) const;
+
         /// Compute and return the AABB of the body by merging all proxy shapes AABBs
         AABB getAABB() const;
 
@@ -301,6 +304,15 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
     return mTransform.getOrientation().getInverse() * worldVector;
 }
 
+/// Test if the collision body overlaps with a given AABB
+/**
+* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
+* @return True if the given AABB overlaps with the AABB of the collision body
+*/
+inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const {
+    return worldAABB.testCollision(getAABB());
+}
+
 }
 
 #endif

src/collision/CollisionDetection.cpp

@@ -35,15 +35,16 @@
 #include <set>
 #include <utility>
 #include <utility>
+#include <unordered_set>
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator)
+CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator)
-                   : mMemoryAllocator(memoryAllocator),
+                   : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
-                     mWorld(world), mBroadPhaseAlgorithm(*this),
+                     mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
                      mIsCollisionShapesAdded(false) {
 
     // Set the default collision dispatch configuration
@@ -61,26 +62,19 @@ void CollisionDetection::computeCollisionDetection() {
     // Compute the broad-phase collision detection
     computeBroadPhase();
 
+    // Compute the middle-phase collision detection
+    computeMiddlePhase();
+    
     // Compute the narrow-phase collision detection
      computeNarrowPhase();
+
+    // Reset the linked list of narrow-phase info
+    mNarrowPhaseInfoList = nullptr;
 }
 
-// Compute the collision detection
+// TODO : Remove this method
-void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
-                                                    const std::set<uint>& shapes1,
-                                                    const std::set<uint>& shapes2) {
-
-    // Compute the broad-phase collision detection
-    computeBroadPhase();
-
-    // Delete all the contact points in the currently overlapping pairs
-    clearContactPoints();
-
-    // Compute the narrow-phase collision detection among given sets of shapes
-    computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
-}
-
 // Report collision between two sets of shapes
+/*
 void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
                                                       const std::set<uint>& shapes1,
                                                       const std::set<uint>& shapes2) {
@@ -126,13 +120,9 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
                 ContactPoint* contactPoint = manifold->getContactPoint(i);
 
                 // Create the contact info object for the contact
-                ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
+                ContactPointInfo contactInfo;
-                                             manifold->getShape1()->getCollisionShape(),
+                contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(),
-                                             manifold->getShape2()->getCollisionShape(),
+                                 contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2());
-                                             contactPoint->getNormal(),
-                                             contactPoint->getPenetrationDepth(),
-                                             contactPoint->getLocalPointOnBody1(),
-                                             contactPoint->getLocalPointOnBody2());
 
                 // Notify the collision callback about this new contact
                 if (callback != nullptr) callback->notifyContact(contactInfo);
@@ -140,6 +130,7 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
         }
     }
 }
+*/
 
 // Compute the broad-phase collision detection
 void CollisionDetection::computeBroadPhase() {
@@ -152,14 +143,14 @@ void CollisionDetection::computeBroadPhase() {
         // Ask the broad-phase to recompute the overlapping pairs of collision
         // shapes. This call can only add new overlapping pairs in the collision
         // detection.
-        mBroadPhaseAlgorithm.computeOverlappingPairs();
+        mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator);
     }
 }
 
-// Compute the narrow-phase collision detection
+// Compute the middle-phase collision detection
-void CollisionDetection::computeNarrowPhase() {
+void CollisionDetection::computeMiddlePhase() {
 
-    PROFILE("CollisionDetection::computeNarrowPhase()");
+    PROFILE("CollisionDetection::computeMiddlePhase()");
 
     // Clear the set of overlapping pairs in narrow-phase contact
     mContactOverlappingPairs.clear();
@@ -212,36 +203,153 @@ void CollisionDetection::computeNarrowPhase() {
         bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
         if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
 
-        // Select the narrow phase algorithm to use according to the two collision shapes
         const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
         const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
+        // If both shapes are convex
+        if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+
+            // No middle-phase is necessary, simply create a narrow phase info
+            // for the narrow-phase collision detection
+            NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
+            mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
+                                   NarrowPhaseInfo(pair, shape1->getCollisionShape(),
+                                   shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
+                                   shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
+                                   shape2->getCachedCollisionData());
+            mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
+
+        }
+        // Concave vs Convex algorithm
+        else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+                 (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+
+            NarrowPhaseInfo* narrowPhaseInfo = nullptr;
+            computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
+
+            // Add all the narrow-phase info object reported by the callback into the
+            // list of all the narrow-phase info object
+            while (narrowPhaseInfo != nullptr) {
+                NarrowPhaseInfo* next = narrowPhaseInfo->next;
+                narrowPhaseInfo->next = mNarrowPhaseInfoList;
+                mNarrowPhaseInfoList = narrowPhaseInfo;
+
+                narrowPhaseInfo = next;
+            }
+        }
+        // Concave vs Concave shape
+        else {
+            // Not handled
+            continue;
+        }
+    }
+}
+
+// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
+void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
+                                                           NarrowPhaseInfo** firstNarrowPhaseInfo) {
+
+    ProxyShape* shape1 = pair->getShape1();
+    ProxyShape* shape2 = pair->getShape2();
+
+    ProxyShape* convexProxyShape;
+    ProxyShape* concaveProxyShape;
+    const ConvexShape* convexShape;
+    const ConcaveShape* concaveShape;
+
+    // Collision shape 1 is convex, collision shape 2 is concave
+    if (shape1->getCollisionShape()->isConvex()) {
+        convexProxyShape = shape1;
+        convexShape = static_cast<const ConvexShape*>(shape1->getCollisionShape());
+        concaveProxyShape = shape2;
+        concaveShape = static_cast<const ConcaveShape*>(shape2->getCollisionShape());
+    }
+    else {  // Collision shape 2 is convex, collision shape 1 is concave
+        convexProxyShape = shape2;
+        convexShape = static_cast<const ConvexShape*>(shape2->getCollisionShape());
+        concaveProxyShape = shape1;
+        concaveShape = static_cast<const ConcaveShape*>(shape1->getCollisionShape());
+    }
+
+    // Set the parameters of the callback object
+    MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape,
+                                                    concaveShape, allocator);
+
+    // Compute the convex shape AABB in the local-space of the convex shape
+    AABB aabb;
+    convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
+
+    // TODO : Implement smooth concave mesh collision somewhere
+
+    // Call the convex vs triangle callback for each triangle of the concave shape
+    concaveShape->testAllTriangles(middlePhaseCallback, aabb);
+
+    // Add all the narrow-phase info object reported by the callback into the
+    // list of all the narrow-phase info object
+    *firstNarrowPhaseInfo = middlePhaseCallback.narrowPhaseInfoList;
+}
+
+// Compute the narrow-phase collision detection
+void CollisionDetection::computeNarrowPhase() {
+
+    PROFILE("CollisionDetection::computeNarrowPhase()");
+
+    const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
+    while (currentNarrowPhaseInfo != nullptr) {
+
+        // Select the narrow phase algorithm to use according to the two collision shapes
+        const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType();
+        const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType();
         const int shape1Index = static_cast<int>(shape1Type);
         const int shape2Index = static_cast<int>(shape2Type);
         NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
 
-        // If there is no collision algorithm between those two kinds of shapes
+        // If there is no collision algorithm between those two kinds of shapes, skip it
-        if (narrowPhaseAlgorithm == nullptr) continue;
+        if (narrowPhaseAlgorithm != nullptr) {
-        
-        // Notify the narrow-phase algorithm about the overlapping pair we are going to test
-        narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
-
-        // Create the CollisionShapeInfo objects
-        CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
-                                      pair, shape1->getCachedCollisionData());
-        CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
-                                      pair, shape2->getCachedCollisionData());
         
             // Use the narrow-phase collision detection algorithm to check
             // if there really is a collision. If a collision occurs, the
             // notifyContact() callback method will be called.
-        narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
+            ContactPointInfo contactPointInfo;
+            if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) {
+
+                // If it is the first contact since the pairs are overlapping
+                if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
+
+                    // Trigger a callback event
+                    if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo);
+                }
+
+                // Create a new contact
+                ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
+                                             ContactPoint(contactPointInfo);
+
+                contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform,
+                                                  currentNarrowPhaseInfo->shape2ToWorldTransform);
+
+                // Add the contact to the contact manifold set of the corresponding overlapping pair
+                currentNarrowPhaseInfo->overlappingPair->addContact(contact);
+
+                // Add the overlapping pair into the set of pairs in contact during narrow-phase
+                overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
+                                                                      currentNarrowPhaseInfo->overlappingPair->getShape2());
+                mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
+
+                // Trigger a callback event for the new contact
+                if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo);
+            }
+        }
+
+        currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
     }
 
     // Add all the contact manifolds (between colliding bodies) to the bodies
     addAllContactManifoldsToBodies();
 }
 
+// TODO : Remove this method
 // Compute the narrow-phase collision detection
+/*
 void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
                                                          const std::set<uint>& shapes1,
                                                          const std::set<uint>& shapes2) {
@@ -326,9 +434,6 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
         // If there is no collision algorithm between those two kinds of shapes
         if (narrowPhaseAlgorithm == nullptr) continue;
 
-        // Notify the narrow-phase algorithm about the overlapping pair we are going to test
-        narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
-
         // Create the CollisionShapeInfo objects
         CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
                                       pair, shape1->getCachedCollisionData());
@@ -345,6 +450,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
     // Add all the contact manifolds (between colliding bodies) to the bodies
     addAllContactManifoldsToBodies();
 }
+*/
 
 // Allow the broadphase to notify the collision detection about an overlapping pair.
 /// This method is called by the broad-phase collision detection algorithm
@@ -352,9 +458,6 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
 
     assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
 
-    // If the two proxy collision shapes are from the same body, skip it
-    if (shape1->getBody()->getID() == shape2->getBody()->getID()) return;
-
     // Check if the collision filtering allows collision between the two shapes
     if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
         (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
@@ -412,40 +515,6 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
     mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
 }
 
-// Called by a narrow-phase collision algorithm when a new contact has been found
-void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
-
-    // If it is the first contact since the pairs are overlapping
-    if (overlappingPair->getNbContactPoints() == 0) {
-
-        // Trigger a callback event
-        if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactInfo);
-    }
-
-    // Create a new contact
-    createContact(overlappingPair, contactInfo);
-
-    // Trigger a callback event for the new contact
-    if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactInfo);
-}
-
-// Create a new contact
-void CollisionDetection::createContact(OverlappingPair* overlappingPair,
-                                       const ContactPointInfo& contactInfo) {
-
-    // Create a new contact
-    ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
-                                 ContactPoint(contactInfo);
-
-    // Add the contact to the contact manifold set of the corresponding overlapping pair
-    overlappingPair->addContact(contact);
-
-    // Add the overlapping pair into the set of pairs in contact during narrow-phase
-    overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
-                                                          overlappingPair->getShape2());
-    mContactOverlappingPairs[pairId] = overlappingPair;
-}
-
 void CollisionDetection::addAllContactManifoldsToBodies() {
 
     // For each overlapping pairs in contact during the narrow-phase
@@ -503,6 +572,464 @@ void CollisionDetection::clearContactPoints() {
     }
 }
 
+// Compute the middle-phase collision detection between two proxy shapes
+NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2) {
+
+    // Create a temporary overlapping pair
+    OverlappingPair pair(shape1, shape2, 0, mMemoryAllocator);
+
+    // -------------------------------------------------------
+
+    const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+    const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
+    NarrowPhaseInfo* narrowPhaseInfo = nullptr;
+
+    // If both shapes are convex
+    if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+
+        // No middle-phase is necessary, simply create a narrow phase info
+        // for the narrow-phase collision detection
+        narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(&pair, shape1->getCollisionShape(),
+                                       shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
+                                       shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
+                                       shape2->getCachedCollisionData());
+
+    }
+    // Concave vs Convex algorithm
+    else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+             (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+
+        // Run the middle-phase collision detection algorithm to find the triangles of the concave
+        // shape we need to use during the narrow-phase collision detection
+        computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo);
+    }
+
+    return nullptr;
+}
+
+// Report all the bodies that overlap with the aabb in parameter
+void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback,
+                                         unsigned short categoryMaskBits) {
+    assert(overlapCallback != nullptr);
+
+    std::unordered_set<bodyindex> reportedBodies;
+
+    // Ask the broad-phase to get all the overlapping shapes
+    LinkedList<int> overlappingNodes(mMemoryAllocator);
+    mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
+
+    // For each overlaping proxy shape
+    LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+    while (element != nullptr) {
+
+        // Get the overlapping proxy shape
+        int broadPhaseId = element->data;
+        ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+
+        CollisionBody* overlapBody = proxyShape->getBody();
+
+        // If the proxy shape is from a body that we have not already reported collision
+        if (reportedBodies.find(overlapBody->getID()) == reportedBodies.end()) {
+
+            // Check if the collision filtering allows collision between the two shapes
+            if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+
+                // Add the body into the set of reported bodies
+                reportedBodies.insert(overlapBody->getID());
+
+                // Notify the overlap to the user
+                overlapCallback->notifyOverlap(overlapBody);
+            }
+        }
+
+        // Go to the next overlapping proxy shape
+        element = element->next;
+    }
+}
+
+// Return true if two bodies overlap
+bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) {
+
+    // For each proxy shape proxy shape of the first body
+    ProxyShape* body1ProxyShape = body1->getProxyShapesList();
+    while (body1ProxyShape != nullptr) {
+
+        AABB aabb1 = body1ProxyShape->getWorldAABB();
+
+        // For each proxy shape of the second body
+        ProxyShape* body2ProxyShape = body2->getProxyShapesList();
+        while (body2ProxyShape != nullptr) {
+
+            AABB aabb2 = body2ProxyShape->getWorldAABB();
+
+            // Test if the AABBs of the two proxy shapes overlap
+            if (aabb1.testCollision(aabb2)) {
+
+                const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
+                const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
+
+                // Compute the middle-phase collision detection between the two shapes
+                NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape);
+
+                bool isColliding = false;
+
+                // For each narrow-phase info object
+                while (narrowPhaseInfo != nullptr) {
+
+                    // If we have not found a collision yet
+                    if (!isColliding) {
+
+                        // Select the narrow phase algorithm to use according to the two collision shapes
+                        const int shape1Index = static_cast<int>(shape1Type);
+                        const int shape2Index = static_cast<int>(shape2Type);
+                        NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+                        // If there is a collision algorithm for those two kinds of shapes
+                        if (narrowPhaseAlgorithm != nullptr) {
+
+                            // Use the narrow-phase collision detection algorithm to check
+                            // if there really is a collision. If a collision occurs, the
+                            // notifyContact() callback method will be called.
+                            ContactPointInfo contactPointInfo;
+                            isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
+                        }
+                    }
+
+                    NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+                    narrowPhaseInfo = narrowPhaseInfo->next;
+
+                    // Release the allocated memory
+                    mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                }
+
+                // Return if we have found a narrow-phase collision
+                if (isColliding) return true;
+            }
+
+            // Go to the next proxy shape
+            body2ProxyShape = body2ProxyShape->getNext();
+        }
+
+        // Go to the next proxy shape
+        body1ProxyShape = body1ProxyShape->getNext();
+    }
+
+    // No overlap has been found
+    return false;
+}
+
+// Report all the bodies that overlap with the body in parameter
+void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback,
+                                     unsigned short categoryMaskBits) {
+
+    assert(overlapCallback != nullptr);
+
+    std::unordered_set<bodyindex> reportedBodies;
+
+    // For each proxy shape proxy shape of the body
+    ProxyShape* bodyProxyShape = body->getProxyShapesList();
+    while (bodyProxyShape != nullptr) {
+
+        // Get the AABB of the shape
+        const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
+
+        // Ask the broad-phase to get all the overlapping shapes
+        LinkedList<int> overlappingNodes(mMemoryAllocator);
+        mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
+
+        const bodyindex bodyId = body->getID();
+
+        // For each overlaping proxy shape
+        LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+        while (element != nullptr) {
+
+            // Get the overlapping proxy shape
+            int broadPhaseId = element->data;
+            ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+
+            // If the proxy shape is from a body that we have not already reported collision and the
+            // two proxy collision shapes are not from the same body
+            if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() &&
+                proxyShape->getBody()->getID() != bodyId) {
+
+                // Check if the collision filtering allows collision between the two shapes
+                if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+
+                    const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
+                    const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
+
+                    // Compute the middle-phase collision detection between the two shapes
+                    NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape);
+
+                    bool isColliding = false;
+
+                    // For each narrow-phase info object
+                    while (narrowPhaseInfo != nullptr) {
+
+                        // If we have not found a collision yet
+                        if (!isColliding) {
+
+                            // Select the narrow phase algorithm to use according to the two collision shapes
+                            const int shape1Index = static_cast<int>(shape1Type);
+                            const int shape2Index = static_cast<int>(shape2Type);
+                            NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+                            // If there is a collision algorithm for those two kinds of shapes
+                            if (narrowPhaseAlgorithm != nullptr) {
+
+                                // Use the narrow-phase collision detection algorithm to check
+                                // if there really is a collision. If a collision occurs, the
+                                // notifyContact() callback method will be called.
+                                ContactPointInfo contactPointInfo;
+                                isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
+                            }
+                        }
+
+                        NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+                        narrowPhaseInfo = narrowPhaseInfo->next;
+
+                        // Release the allocated memory
+                        mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                    }
+
+                    // Return if we have found a narrow-phase collision
+                    if (isColliding) {
+
+                        CollisionBody* overlapBody = proxyShape->getBody();
+
+                        // Add the body into the set of reported bodies
+                        reportedBodies.insert(overlapBody->getID());
+
+                        // Notify the overlap to the user
+                        overlapCallback->notifyOverlap(overlapBody);
+                    }
+                }
+            }
+
+            // Go to the next overlapping proxy shape
+            element = element->next;
+        }
+
+        // Go to the next proxy shape
+        bodyProxyShape = bodyProxyShape->getNext();
+    }
+}
+
+// Test and report collisions between two bodies
+void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* collisionCallback) {
+
+    assert(collisionCallback != nullptr);
+
+    // For each proxy shape proxy shape of the first body
+    ProxyShape* body1ProxyShape = body1->getProxyShapesList();
+    while (body1ProxyShape != nullptr) {
+
+        AABB aabb1 = body1ProxyShape->getWorldAABB();
+
+        // For each proxy shape of the second body
+        ProxyShape* body2ProxyShape = body2->getProxyShapesList();
+        while (body2ProxyShape != nullptr) {
+
+            AABB aabb2 = body2ProxyShape->getWorldAABB();
+
+            // Test if the AABBs of the two proxy shapes overlap
+            if (aabb1.testCollision(aabb2)) {
+
+                // Compute the middle-phase collision detection between the two shapes
+                NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape);
+
+                const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
+                const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
+
+                // For each narrow-phase info object
+                while (narrowPhaseInfo != nullptr) {
+
+                    // Select the narrow phase algorithm to use according to the two collision shapes
+                    const int shape1Index = static_cast<int>(shape1Type);
+                    const int shape2Index = static_cast<int>(shape2Type);
+                    NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+                    // If there is a collision algorithm for those two kinds of shapes
+                    if (narrowPhaseAlgorithm != nullptr) {
+
+                        // Use the narrow-phase collision detection algorithm to check
+                        // if there really is a collision. If a collision occurs, the
+                        // notifyContact() callback method will be called.
+                        ContactPointInfo contactPointInfo;
+                        if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+
+                            CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2,
+                                                                                   body1ProxyShape, body2ProxyShape);
+
+                            // Report the contact to the user
+                            collisionCallback->notifyContact(collisionInfo);
+                        }
+                    }
+
+                    NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+                    narrowPhaseInfo = narrowPhaseInfo->next;
+
+                    // Release the allocated memory
+                    mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                }
+            }
+
+            // Go to the next proxy shape
+            body2ProxyShape = body2ProxyShape->getNext();
+        }
+
+        // Go to the next proxy shape
+        body1ProxyShape = body1ProxyShape->getNext();
+    }
+}
+
+// Test and report collisions between a body and all the others bodies of the world
+void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) {
+
+    assert(callback != nullptr);
+
+    // For each proxy shape proxy shape of the body
+    ProxyShape* bodyProxyShape = body->getProxyShapesList();
+    while (bodyProxyShape != nullptr) {
+
+        // Get the AABB of the shape
+        const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
+
+        // Ask the broad-phase to get all the overlapping shapes
+        LinkedList<int> overlappingNodes(mMemoryAllocator);
+        mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
+
+        const bodyindex bodyId = body->getID();
+
+        // For each overlaping proxy shape
+        LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+        while (element != nullptr) {
+
+            // Get the overlapping proxy shape
+            int broadPhaseId = element->data;
+            ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+
+            // If the two proxy collision shapes are not from the same body
+            if (proxyShape->getBody()->getID() != bodyId) {
+
+                // Check if the collision filtering allows collision between the two shapes
+                if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+
+                    const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
+                    const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
+
+                    // Compute the middle-phase collision detection between the two shapes
+                    NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape);
+
+                    // For each narrow-phase info object
+                    while (narrowPhaseInfo != nullptr) {
+
+                        // Select the narrow phase algorithm to use according to the two collision shapes
+                        const int shape1Index = static_cast<int>(shape1Type);
+                        const int shape2Index = static_cast<int>(shape2Type);
+                        NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+                        // If there is a collision algorithm for those two kinds of shapes
+                        if (narrowPhaseAlgorithm != nullptr) {
+
+                            // Use the narrow-phase collision detection algorithm to check
+                            // if there really is a collision. If a collision occurs, the
+                            // notifyContact() callback method will be called.
+                            ContactPointInfo contactPointInfo;
+                            if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+
+                                CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body,
+                                                                                       proxyShape->getBody(), bodyProxyShape,
+                                                                                       proxyShape);
+
+                                // Report the contact to the user
+                                callback->notifyContact(collisionInfo);
+                            }
+                        }
+
+                        NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+                        narrowPhaseInfo = narrowPhaseInfo->next;
+
+                        // Release the allocated memory
+                        mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                    }
+                }
+            }
+
+            // Go to the next overlapping proxy shape
+            element = element->next;
+        }
+
+        // Go to the next proxy shape
+        bodyProxyShape = bodyProxyShape->getNext();
+    }
+}
+
+// Test and report collisions between all shapes of the world
+void CollisionDetection::testCollision(CollisionCallback* callback) {
+
+    assert(callback != nullptr);
+
+    // Compute the broad-phase collision detection
+    computeBroadPhase();
+
+    // For each possible collision pair of bodies
+    map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+
+        OverlappingPair* pair = it->second;
+
+        ProxyShape* shape1 = pair->getShape1();
+        ProxyShape* shape2 = pair->getShape2();
+
+        // Check if the collision filtering allows collision between the two shapes and
+        // that the two shapes are still overlapping.
+        if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
+             (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0) &&
+             mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+
+            // Compute the middle-phase collision detection between the two shapes
+            NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(shape1, shape2);
+
+            const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+            const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
+            // For each narrow-phase info object
+            while (narrowPhaseInfo != nullptr) {
+
+                // Select the narrow phase algorithm to use according to the two collision shapes
+                const int shape1Index = static_cast<int>(shape1Type);
+                const int shape2Index = static_cast<int>(shape2Type);
+                NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+                // If there is a collision algorithm for those two kinds of shapes
+                if (narrowPhaseAlgorithm != nullptr) {
+
+                    // Use the narrow-phase collision detection algorithm to check
+                    // if there really is a collision. If a collision occurs, the
+                    // notifyContact() callback method will be called.
+                    ContactPointInfo contactPointInfo;
+                    if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+
+                        CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(),
+                                                                               shape2->getBody(), shape1, shape2);
+
+                        // Report the contact to the user
+                        callback->notifyContact(collisionInfo);
+                    }
+                }
+
+                NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+                narrowPhaseInfo = narrowPhaseInfo->next;
+
+                // Release the allocated memory
+                mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+            }
+        }
+    }
+}
+
 // Fill-in the collision detection matrix
 void CollisionDetection::fillInCollisionMatrix() {
 
@@ -523,9 +1050,3 @@ EventListener* CollisionDetection::getWorldEventListener() {
 PoolAllocator& CollisionDetection::getWorldMemoryAllocator() {
   return mWorld->mPoolAllocator;
 }
-
-// Called by a narrow-phase collision algorithm when a new contact has been found
-void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair,
-                           const ContactPointInfo& contactInfo) {
-    mCollisionCallback->notifyContact(contactInfo);
-}

src/collision/CollisionDetection.h

@@ -33,6 +33,7 @@
 #include "engine/EventListener.h"
 #include "narrowphase/DefaultCollisionDispatch.h"
 #include "memory/PoolAllocator.h"
+#include "memory/SingleFrameAllocator.h"
 #include "constraint/ContactPoint.h"
 #include <vector>
 #include <set>
@@ -46,29 +47,7 @@ namespace reactphysics3d {
 class BroadPhaseAlgorithm;
 class CollisionWorld;
 class CollisionCallback;
-
+class OverlapCallback;
-// Class TestCollisionBetweenShapesCallback
-class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
-
-    private:
-
-        CollisionCallback* mCollisionCallback;
-
-    public:
-
-        // Constructor
-        TestCollisionBetweenShapesCallback(CollisionCallback* callback)
-            : mCollisionCallback(callback) {
-
-        }
-
-        // Destructor
-        virtual ~TestCollisionBetweenShapesCallback() { }
-
-        // Called by a narrow-phase collision algorithm when a new contact has been found
-        virtual void notifyContact(OverlappingPair* overlappingPair,
-                                   const ContactPointInfo& contactInfo) override;
-};
 
 // Class CollisionDetection
 /**
@@ -77,7 +56,7 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
  * collide and then we run a narrow-phase algorithm to compute the
  * collision contacts between bodies.
  */
-class CollisionDetection : public NarrowPhaseCallback {
+class CollisionDetection {
 
     private :
 
@@ -95,9 +74,15 @@ class CollisionDetection : public NarrowPhaseCallback {
         /// Reference to the memory allocator
         PoolAllocator& mMemoryAllocator;
 
+        /// Reference to the single frame memory allocator
+        SingleFrameAllocator& mSingleFrameAllocator;
+
         /// Pointer to the physics world
         CollisionWorld* mWorld;
 
+        /// Pointer to the first narrow-phase info of the linked list
+        NarrowPhaseInfo* mNarrowPhaseInfoList;
+
         /// Broad-phase overlapping pairs
         std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
 
@@ -111,6 +96,7 @@ class CollisionDetection : public NarrowPhaseCallback {
         // TODO : Delete this
         GJKAlgorithm mNarrowPhaseGJKAlgorithm;
 
+        // TODO : Maybe delete this set (what is the purpose ?)
         /// Set of pair of bodies that cannot collide between each other
         std::set<bodyindexpair> mNoCollisionPairs;
 
@@ -122,6 +108,9 @@ class CollisionDetection : public NarrowPhaseCallback {
         /// Compute the broad-phase collision detection
         void computeBroadPhase();
 
+        /// Compute the middle-phase collision detection
+        void computeMiddlePhase();
+
         /// Compute the narrow-phase collision detection
         void computeNarrowPhase();
 
@@ -138,12 +127,19 @@ class CollisionDetection : public NarrowPhaseCallback {
         /// Add all the contact manifold of colliding pairs to their bodies
         void addAllContactManifoldsToBodies();
 
+        /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
+        void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
+                                               NarrowPhaseInfo** firstNarrowPhaseInfo);
+
+        /// Compute the middle-phase collision detection between two proxy shapes
+        NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2);
+   
     public :
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator);
+        CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator);
 
         /// Destructor
         ~CollisionDetection() = default;
@@ -183,35 +179,42 @@ class CollisionDetection : public NarrowPhaseCallback {
         /// Compute the collision detection
         void computeCollisionDetection();
 
-        /// Compute the collision detection
+        // TODO : Remove this method
-        void testCollisionBetweenShapes(CollisionCallback* callback,
-                                        const std::set<uint>& shapes1,
-                                        const std::set<uint>& shapes2);
-
         /// Report collision between two sets of shapes
-        void reportCollisionBetweenShapes(CollisionCallback* callback,
+        //void reportCollisionBetweenShapes(CollisionCallback* callback,
-                                          const std::set<uint>& shapes1,
+        //                                  const std::set<uint>& shapes1,
-                                          const std::set<uint>& shapes2) ;
+        //                                  const std::set<uint>& shapes2) ;
 
         /// Ray casting method
         void raycast(RaycastCallback* raycastCallback, const Ray& ray,
                      unsigned short raycastWithCategoryMaskBits) const;
 
-        /// Test if the AABBs of two bodies overlap
+        /// Report all the bodies that overlap with the aabb in parameter
-        bool testAABBOverlap(const CollisionBody* body1,
+        void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-                             const CollisionBody* body2) const;
 
-        /// Test if the AABBs of two proxy shapes overlap
+        /// Return true if two bodies overlap
-        bool testAABBOverlap(const ProxyShape* shape1,
+        bool testOverlap(CollisionBody* body1, CollisionBody* body2);
-                             const ProxyShape* shape2) const;
+
+        /// Report all the bodies that overlap with the body in parameter
+        void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
+
+        /// Test and report collisions between two bodies
+        void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+
+        /// Test and report collisions between a body and all the others bodies of the world
+        void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
+
+        /// Test and report collisions between all shapes of the world
+        void testCollision(CollisionCallback* callback);
 
         /// Allow the broadphase to notify the collision detection about an overlapping pair.
         void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
 
+        // TODO : Remove this method
         /// Compute the narrow-phase collision detection
-        void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+        //void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
-                                             const std::set<uint>& shapes1,
+        //                                     const std::set<uint>& shapes1,
-                                             const std::set<uint>& shapes2);
+        //                                     const std::set<uint>& shapes2);
 
         /// Return a pointer to the world
         CollisionWorld* getWorld();
@@ -222,12 +225,6 @@ class CollisionDetection : public NarrowPhaseCallback {
         /// Return a reference to the world memory allocator
         PoolAllocator& getWorldMemoryAllocator();
 
-        /// Called by a narrow-phase collision algorithm when a new contact has been found
-        virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override;
-
-        /// Create a new contact
-        void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
-
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
@@ -244,8 +241,6 @@ inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(Collision
 inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
     mCollisionDispatch = collisionDispatch;
 
-    mCollisionDispatch->init(this, &mMemoryAllocator);
-
     // Fill-in the collision matrix with the new algorithms to use
     fillInCollisionMatrix();
 }
@@ -299,18 +294,6 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
     mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
 }
 
-// Test if the AABBs of two proxy shapes overlap
-inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
-                                                const ProxyShape* shape2) const {
-
-    // If one of the shape's body is not active, we return no overlap
-    if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
-        return false;
-    }
-
-    return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
-}
-
 // Return a pointer to the world
 inline CollisionWorld* CollisionDetection::getWorld() {
     return mWorld;

src/collision/ContactManifold.cpp

@@ -110,12 +110,9 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
 
     // Update the world coordinates and penetration depth of the contact points in the manifold
     for (uint i=0; i<mNbContactPoints; i++) {
-        mContactPoints[i]->setWorldPointOnBody1(transform1 *
+
-                                                mContactPoints[i]->getLocalPointOnBody1());
+        mContactPoints[i]->updateWorldContactPoints(transform1, transform2);
-        mContactPoints[i]->setWorldPointOnBody2(transform2 *
+        mContactPoints[i]->updatePenetrationDepth();
-                                                mContactPoints[i]->getLocalPointOnBody2());
-        mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
-                  mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
     }
 
     const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *

src/collision/ContactManifoldSet.cpp

@@ -33,7 +33,7 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
                                        PoolAllocator& memoryAllocator, int nbMaxManifolds)
                    : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
                      mShape2(shape2), mMemoryAllocator(memoryAllocator) {
-    assert(nbMaxManifolds >= 1);
+
 }
 
 // Destructor

src/collision/NarrowPhaseInfo.h

@@ -23,8 +23,8 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_H
-#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+#define REACTPHYSICS3D_NARROW_PHASE_INFO_H
 
 // Libraries
 #include "shapes/CollisionShape.h"
@@ -34,37 +34,48 @@ namespace reactphysics3d {
 
 class OverlappingPair;
 
-// Class CollisionShapeInfo
+// Class NarrowPhaseInfo
 /**
  * This structure regroups different things about a collision shape. This is
  * used to pass information about a collision shape to a collision algorithm.
  */
-struct CollisionShapeInfo {
+struct NarrowPhaseInfo {
 
     public:
 
         /// Broadphase overlapping pair
         OverlappingPair* overlappingPair;
 
-        /// Proxy shape
+        /// Pointer to the first collision shape to test collision with
-        ProxyShape* proxyShape;
+        const CollisionShape* collisionShape1;
 
-        /// Pointer to the collision shape
+        /// Pointer to the second collision shape to test collision with
-        const CollisionShape* collisionShape;
+        const CollisionShape* collisionShape2;
 
-        /// Transform that maps from collision shape local-space to world-space
+        /// Transform that maps from collision shape 1 local-space to world-space
-        Transform shapeToWorldTransform;
+        Transform shape1ToWorldTransform;
+
+        /// Transform that maps from collision shape 2 local-space to world-space
+        Transform shape2ToWorldTransform;
 
         /// Cached collision data of the proxy shape
-        void** cachedCollisionData;
+        // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
+        void** cachedCollisionData1;
+
+        /// Cached collision data of the proxy shape
+        // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
+        void** cachedCollisionData2;
+
+        /// Pointer to the next element in the linked list
+        NarrowPhaseInfo* next;
 
         /// Constructor
-        CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
+        NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
-                           const Transform& shapeLocalToWorldTransform, OverlappingPair* pair,
+                        const CollisionShape* shape2, const Transform& shape1Transform,
-                           void** cachedData)
+                        const Transform& shape2Transform, void** cachedData1, void** cachedData2)
-              : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape),
+              : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
-                shapeToWorldTransform(shapeLocalToWorldTransform),
+                shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
-                cachedCollisionData(cachedData) {
+                cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) {
 
         }
 };

src/collision/ProxyShape.h

@@ -126,6 +126,12 @@ class ProxyShape {
         /// Return the local to world transform
         const Transform getLocalToWorldTransform() const;
 
+        /// Return the AABB of the proxy shape in world-space
+        const AABB getWorldAABB() const;
+
+        /// Test if the proxy shape overlaps with a given AABB
+        bool testAABBOverlap(const AABB& worldAABB) const;
+
         /// Return true if a point is inside the collision shape
         bool testPointInside(const Vector3& worldPoint);
 
@@ -249,6 +255,16 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
     return mBody->mTransform * mLocalToBodyTransform;
 }
 
+// Return the AABB of the proxy shape in world-space
+/**
+ * @return The AABB of the proxy shape in world-space
+ */
+inline const AABB ProxyShape::getWorldAABB() const {
+    AABB aabb;
+    mCollisionShape->computeAABB(aabb, getLocalToWorldTransform());
+    return aabb;
+}
+
 // Return the next proxy shape in the linked list of proxy shapes
 /**
  * @return Pointer to the next proxy shape in the linked list of proxy shapes
@@ -320,6 +336,15 @@ inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
     mBody->updateProxyShapeInBroadPhase(this, true);
 }
 
+/// Test if the proxy shape overlaps with a given AABB
+/**
+* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
+* @return True if the given AABB overlaps with the AABB of the collision body
+*/
+inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const {
+    return worldAABB.testCollision(getWorldAABB());
+}
+
 }
 
 #endif

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -162,12 +162,25 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
     }
 }
 
+void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb,
+                                                             LinkedList<int>& overlappingNodes) const {
+
+    AABBOverlapCallback callback(overlappingNodes);
+
+    // Ask the dynamic AABB tree to report all collision shapes that overlap with this AABB
+    mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, callback);
+}
+
 // Compute all the overlapping pairs of collision shapes
-void BroadPhaseAlgorithm::computeOverlappingPairs() {
+void BroadPhaseAlgorithm::computeOverlappingPairs(Allocator& allocator) {
+
+    // TODO : Try to see if we can allocate potential pairs in single frame allocator
 
     // Reset the potential overlapping pairs
     mNbPotentialPairs = 0;
 
+    LinkedList<int> overlappingNodes(allocator);
+
     // For all collision shapes that have moved (or have been created) during the
     // last simulation step
     for (uint i=0; i<mNbMovedShapes; i++) {
@@ -175,7 +188,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
 
         if (shapeID == -1) continue;
 
-        AABBOverlapCallback callback(*this, shapeID);
+        AABBOverlapCallback callback(overlappingNodes);
 
         // Get the AABB of the shape
         const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
@@ -184,6 +197,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
         // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
         // by the dynamic AABB tree for each potential overlapping pair.
         mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
+
+        // Add the potential overlapping pairs
+        addOverlappingNodes(shapeID, overlappingNodes);
+
+        // Remove all the elements of the linked list of overlapping nodes
+        overlappingNodes.reset();
     }
 
     // Reset the array of collision shapes that have move (or have been created) during the
@@ -208,8 +227,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
         ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
         ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
 
+        // If the two proxy collision shapes are from the same body, skip it
+        if (shape1->getBody()->getID() != shape2->getBody()->getID()) {
+
             // Notify the collision detection about the overlapping pair
             mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
+        }
 
         // Skip the duplicate overlapping pairs
         while (i < mNbPotentialPairs) {
@@ -241,10 +264,14 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
 }
 
 // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
-void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
+void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedList<int>& overlappingNodes) {
+
+    // For each overlapping node in the linked list
+    LinkedList<int>::ListElement* elem = overlappingNodes.getListHead();
+    while (elem != nullptr) {
 
         // If both the nodes are the same, we do not create store the overlapping pair
-    if (node1ID == node2ID) return;
+        if (referenceNodeId != elem->data) {
 
             // If we need to allocate more memory for the array of potential overlapping pairs
             if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
@@ -259,16 +286,19 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
             }
 
             // Add the new potential pair into the array of potential overlapping pairs
-    mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
+            mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(referenceNodeId, elem->data);
-    mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
+            mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(referenceNodeId, elem->data);
             mNbPotentialPairs++;
+        }
+
+        elem = elem->next;
+    }
 }
 
 // Called when a overlapping node has been found during the call to
 // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
 void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
-
+    mOverlappingNodes.insert(nodeId);
-    mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
 }
 
 // Called for a broad-phase shape that has to be tested for raycast

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -32,6 +32,7 @@
 #include "collision/ProxyShape.h"
 #include "DynamicAABBTree.h"
 #include "engine/Profiler.h"
+#include "containers/LinkedList.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -66,15 +67,13 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
 
     private:
 
-        BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
-
-        int mReferenceNodeId;
-
     public:
 
+        LinkedList<int>& mOverlappingNodes;
+
         // Constructor
-        AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
+        AABBOverlapCallback(LinkedList<int>& overlappingNodes)
-             : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
+             : mOverlappingNodes(overlappingNodes) {
 
         }
 
@@ -197,15 +196,24 @@ class BroadPhaseAlgorithm {
         /// step and that need to be tested again for broad-phase overlapping.
         void removeMovedCollisionShape(int broadPhaseID);
 
-        /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
+        /// Add potential overlapping pairs in the dynamic AABB tree
-        void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
+        void addOverlappingNodes(int broadPhaseId1, const LinkedList<int>& overlappingNodes);
+
+        /// Report all the shapes that are overlapping with a given AABB
+        void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList<int>& overlappingNodes) const;
 
         /// Compute all the overlapping pairs of collision shapes
-        void computeOverlappingPairs();
+        void computeOverlappingPairs(Allocator& allocator);
+
+        /// Return the proxy shape corresponding to the broad-phase node id in parameter
+        ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
 
         /// Return true if the two broad-phase collision shapes are overlapping
         bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
 
+        /// Return the fat AABB of a given broad-phase shape
+        const AABB& getFatAABB(int broadPhaseId) const;
+
         /// Ray casting method
         void raycast(const Ray& ray, RaycastTest& raycastTest,
                      unsigned short raycastWithCategoryMaskBits) const;
@@ -232,6 +240,11 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
     return aabb1.testCollision(aabb2);
 }
 
+// Return the fat AABB of a given broad-phase shape
+inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const  {
+    return mDynamicAABBTree.getFatAABB(broadPhaseId);
+}
+
 // Ray casting method
 inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
                                          unsigned short raycastWithCategoryMaskBits) const {
@@ -243,6 +256,11 @@ inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTes
     mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
 }
 
+// Return the proxy shape corresponding to the broad-phase node id in parameter
+inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPhaseId) const {
+    return static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(broadPhaseId));
+}
+
 }
 
 #endif

src/collision/broadphase/DynamicAABBTree.cpp

@@ -26,7 +26,7 @@
 // Libraries
 #include "DynamicAABBTree.h"
 #include "BroadPhaseAlgorithm.h"
-#include "memory/Stack.h"
+#include "containers/Stack.h"
 #include "engine/Profiler.h"
 
 using namespace reactphysics3d;

src/collision/narrowphase/CollisionDispatch.h

@@ -49,11 +49,6 @@ class CollisionDispatch {
         /// Destructor
         virtual ~CollisionDispatch() = default;
 
-        /// Initialize the collision dispatch configuration
-        virtual void init(CollisionDetection* collisionDetection,
-                          PoolAllocator* memoryAllocator) {
-
-        }
 
         /// Select and return the narrow-phase collision detection algorithm to
         /// use between two types of collision shapes.

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -33,94 +33,111 @@
 
 using namespace reactphysics3d;
 
-// Return true and compute a contact info if the two bounding volumes collide
+// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
-void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) {
-                                             const CollisionShapeInfo& shape2Info,
-                                             NarrowPhaseCallback* narrowPhaseCallback) {
-
-    ProxyShape* convexProxyShape;
-    ProxyShape* concaveProxyShape;
-    const ConvexShape* convexShape;
-    const ConcaveShape* concaveShape;
-
-    // Collision shape 1 is convex, collision shape 2 is concave
-    if (shape1Info.collisionShape->isConvex()) {
-        convexProxyShape = shape1Info.proxyShape;
-        convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
-        concaveProxyShape = shape2Info.proxyShape;
-        concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
-    }
-    else {  // Collision shape 2 is convex, collision shape 1 is concave
-        convexProxyShape = shape2Info.proxyShape;
-        convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
-        concaveProxyShape = shape1Info.proxyShape;
-        concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
-    }
-
-    // Set the parameters of the callback object
-    ConvexVsTriangleCallback convexVsTriangleCallback;
-    convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
-    convexVsTriangleCallback.setConvexShape(convexShape);
-    convexVsTriangleCallback.setConcaveShape(concaveShape);
-    convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
-    convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
-
-    // Compute the convex shape AABB in the local-space of the convex shape
-    AABB aabb;
-    convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
-
-    // If smooth mesh collision is enabled for the concave mesh
-    if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
-
-        std::vector<SmoothMeshContactInfo> contactPoints;
-
-        SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
-
-        convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
-
-        // Call the convex vs triangle callback for each triangle of the concave shape
-        concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
-
-        // Run the smooth mesh collision algorithm
-        processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
-    }
-    else {
-
-        convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
-
-        // Call the convex vs triangle callback for each triangle of the concave shape
-        concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
-    }
-}
-
-// Test collision between a triangle and the convex mesh shape
-void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
 
     // Create a triangle collision shape
     decimal margin = mConcaveShape->getTriangleMargin();
-    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+    TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
+                                   TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
 
-    // Select the collision algorithm to use between the triangle and the convex shape
+    // Create a narrow phase info for the narrow-phase collision detection
-    NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
+    NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
-                                                                            mConvexShape->getType());
+    narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
-
+                           NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
-    // If there is no collision algorithm between those two kinds of shapes
+                           triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
-    if (algo == nullptr) return;
+                           mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
-
+                           mConcaveProxyShape->getCachedCollisionData());
-    // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+    narrowPhaseInfoList->next = firstNarrowPhaseInfo;
-    algo->setCurrentOverlappingPair(mOverlappingPair);
-
-    // Create the CollisionShapeInfo objects
-    CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
-                                       mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
-    CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
-                                        mConcaveProxyShape->getLocalToWorldTransform(),
-                                        mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
-
-    // Use the collision algorithm to test collision between the triangle and the other convex shape
-    algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
 }
 
+// Return true and compute a contact info if the two bounding volumes collide
+void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+                                             NarrowPhaseCallback* narrowPhaseCallback) {
+
+//    ProxyShape* convexProxyShape;
+//    ProxyShape* concaveProxyShape;
+//    const ConvexShape* convexShape;
+//    const ConcaveShape* concaveShape;
+
+//    // Collision shape 1 is convex, collision shape 2 is concave
+//    if (shape1Info.collisionShape->isConvex()) {
+//        convexProxyShape = shape1Info.proxyShape;
+//        convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+//        concaveProxyShape = shape2Info.proxyShape;
+//        concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
+//    }
+//    else {  // Collision shape 2 is convex, collision shape 1 is concave
+//        convexProxyShape = shape2Info.proxyShape;
+//        convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+//        concaveProxyShape = shape1Info.proxyShape;
+//        concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
+//    }
+
+//    // Set the parameters of the callback object
+//    ConvexVsTriangleCallback convexVsTriangleCallback;
+//    convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
+//    convexVsTriangleCallback.setConvexShape(convexShape);
+//    convexVsTriangleCallback.setConcaveShape(concaveShape);
+//    convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
+//    convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
+
+//    // Compute the convex shape AABB in the local-space of the convex shape
+//    AABB aabb;
+//    convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
+
+//    // If smooth mesh collision is enabled for the concave mesh
+//    if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
+
+//        std::vector<SmoothMeshContactInfo> contactPoints;
+
+//        SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
+
+//        convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
+
+//        // Call the convex vs triangle callback for each triangle of the concave shape
+//        concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
+
+//        // Run the smooth mesh collision algorithm
+//        processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
+//    }
+//    else {
+
+//        convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
+
+//        // Call the convex vs triangle callback for each triangle of the concave shape
+//        concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
+//    }
+}
+
+//// Test collision between a triangle and the convex mesh shape
+//void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
+
+//    // Create a triangle collision shape
+//    decimal margin = mConcaveShape->getTriangleMargin();
+//    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+
+//    // Select the collision algorithm to use between the triangle and the convex shape
+//    NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
+//                                                                            mConvexShape->getType());
+
+//    // If there is no collision algorithm between those two kinds of shapes
+//    if (algo == nullptr) return;
+
+//    // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+//    algo->setCurrentOverlappingPair(mOverlappingPair);
+
+//    // Create the CollisionShapeInfo objects
+//    CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
+//                                       mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
+//    CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
+//                                        mConcaveProxyShape->getLocalToWorldTransform(),
+//                                        mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
+
+//    // Use the collision algorithm to test collision between the triangle and the other convex shape
+//    algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
+//}
+
 // Process the concave triangle mesh collision using the smooth mesh collision algorithm described
 // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
 // issue with some internal edges.
@@ -246,36 +263,37 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi
     return false;
 }
 
-// Called by a narrow-phase collision algorithm when a new contact has been found
-void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
-                                                       const ContactPointInfo& contactInfo) {
-    Vector3 triangleVertices[3];
-    bool isFirstShapeTriangle;
 
-    // If the collision shape 1 is the triangle
+//// Called by a narrow-phase collision algorithm when a new contact has been found
-    if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) {
+//void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
-        assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE);
+//                                                       const ContactPointInfo& contactInfo) {
+//    Vector3 triangleVertices[3];
+//    bool isFirstShapeTriangle;
 
-        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
+//    // If the collision shape 1 is the triangle
-        triangleVertices[0] = triangleShape->getVertex(0);
+//    if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) {
-        triangleVertices[1] = triangleShape->getVertex(1);
+//        assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE);
-        triangleVertices[2] = triangleShape->getVertex(2);
 
-        isFirstShapeTriangle = true;
+//        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
-    }
+//        triangleVertices[0] = triangleShape->getVertex(0);
-    else {  // If the collision shape 2 is the triangle
+//        triangleVertices[1] = triangleShape->getVertex(1);
-        assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE);
+//        triangleVertices[2] = triangleShape->getVertex(2);
 
-        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
+//        isFirstShapeTriangle = true;
-        triangleVertices[0] = triangleShape->getVertex(0);
+//    }
-        triangleVertices[1] = triangleShape->getVertex(1);
+//    else {  // If the collision shape 2 is the triangle
-        triangleVertices[2] = triangleShape->getVertex(2);
+//        assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE);
 
-        isFirstShapeTriangle = false;
+//        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
-    }
+//        triangleVertices[0] = triangleShape->getVertex(0);
-    SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
+//        triangleVertices[1] = triangleShape->getVertex(1);
+//        triangleVertices[2] = triangleShape->getVertex(2);
 
-    // Add the narrow-phase contact into the list of contact to process for
+//        isFirstShapeTriangle = false;
-    // smooth mesh collision
+//    }
-    mContactPoints.push_back(smoothContactInfo);
+//    SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
-}
+
+//    // Add the narrow-phase contact into the list of contact to process for
+//    // smooth mesh collision
+//    mContactPoints.push_back(smoothContactInfo);
+//}

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -30,6 +30,7 @@
 #include "NarrowPhaseAlgorithm.h"
 #include "collision/shapes/ConvexShape.h"
 #include "collision/shapes/ConcaveShape.h"
+#include "memory/SingleFrameAllocator.h"
 #include <unordered_map>
 
 /// Namespace ReactPhysics3D
@@ -37,73 +38,43 @@ namespace reactphysics3d {
 
 // Class ConvexVsTriangleCallback
 /**
- * This class is used to encapsulate a callback method for
+ * This class is used to report a collision between the triangle
- * collision detection between the triangle of a concave mesh shape
+ * of a concave mesh shape and a convex shape during the
- * and a convex shape.
+ * middle-phase algorithm
  */
-class ConvexVsTriangleCallback : public TriangleCallback {
+class MiddlePhaseTriangleCallback : public TriangleCallback {
 
     protected:
 
-        /// Pointer to the collision detection object
-        CollisionDetection* mCollisionDetection;
-
-        /// Narrow-phase collision callback
-        NarrowPhaseCallback* mNarrowPhaseCallback;
-
-        /// Convex collision shape to test collision with
-        const ConvexShape* mConvexShape;
-
-        /// Concave collision shape
-        const ConcaveShape* mConcaveShape;
-
-        /// Proxy shape of the convex collision shape
-        ProxyShape* mConvexProxyShape;
-
-        /// Proxy shape of the concave collision shape
-        ProxyShape* mConcaveProxyShape;
-
         /// Broadphase overlapping pair
         OverlappingPair* mOverlappingPair;
 
-        /// Used to sort ContactPointInfos according to their penetration depth
+        /// Pointer to the concave proxy shape
-        static bool contactsDepthCompare(const ContactPointInfo& contact1,
+        ProxyShape* mConcaveProxyShape;
-                                         const ContactPointInfo& contact2);
+
+        /// Pointer to the convex proxy shape
+        ProxyShape* mConvexProxyShape;
+
+        /// Pointer to the concave collision shape
+        const ConcaveShape* mConcaveShape;
+
+        /// Reference to the single-frame memory allocator
+        Allocator& mAllocator;
 
     public:
 
-        /// Destructor
+        /// Pointer to the first element of the linked-list of narrow-phase info
-        virtual ~ConvexVsTriangleCallback() override = default;
+        NarrowPhaseInfo* narrowPhaseInfoList;
 
-        /// Set the collision detection pointer
+        /// Constructor
-        void setCollisionDetection(CollisionDetection* collisionDetection) {
+        MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
-            mCollisionDetection = collisionDetection;
+                                    ProxyShape* concaveProxyShape,
-        }
+                                    ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
+                                    Allocator& allocator)
+            :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
+             mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
+             mAllocator(allocator), narrowPhaseInfoList(nullptr) {
 
-        /// Set the narrow-phase collision callback
-        void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
-            mNarrowPhaseCallback = callback;
-        }
-
-        /// Set the convex collision shape to test collision with
-        void setConvexShape(const ConvexShape* convexShape) {
-            mConvexShape = convexShape;
-        }
-
-        /// Set the concave collision shape
-        void setConcaveShape(const ConcaveShape* concaveShape) {
-            mConcaveShape = concaveShape;
-        }
-
-        /// Set the broadphase overlapping pair
-        void setOverlappingPair(OverlappingPair* overlappingPair) {
-            mOverlappingPair = overlappingPair;
-        }
-
-        /// Set the proxy shapes of the two collision shapes
-        void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
-            mConvexProxyShape = convexProxyShape;
-            mConcaveProxyShape = concaveProxyShape;
         }
 
         /// Test collision between a triangle and the convex mesh shape
@@ -148,13 +119,14 @@ struct ContactsDepthCompare {
 //    return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
 //}
 
+// TODO : Delete this
 // Class SmoothCollisionNarrowPhaseCallback
 /**
  * This class is used as a narrow-phase callback to get narrow-phase contacts
  * of the concave triangle mesh to temporary store them in order to be used in
  * the smooth mesh collision algorithm if this one is enabled.
  */
-class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
+class SmoothCollisionNarrowPhaseCallback {
 
     private:
 
@@ -169,13 +141,9 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
 
         }
 
-
-        /// Called by a narrow-phase collision algorithm when a new contact has been found
-        virtual void notifyContact(OverlappingPair* overlappingPair,
-                                   const ContactPointInfo& contactInfo) override;
-
 };
 
+// TODO : Delete this
 // Class ConcaveVsConvexAlgorithm
 /**
  * This class is used to compute the narrow-phase collision detection
@@ -183,7 +151,7 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
  * to use the GJK collision detection algorithm to compute the collision between
  * the convex shape and each of the triangles in the concave shape.
  */
-class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
+class ConcaveVsConvexAlgorithm {
 
     protected :
 
@@ -212,7 +180,7 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
         ConcaveVsConvexAlgorithm() = default;
 
         /// Destructor
-        virtual ~ConcaveVsConvexAlgorithm() override = default;
+        ~ConcaveVsConvexAlgorithm() = default;
 
         /// Private copy-constructor
         ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete;
@@ -221,9 +189,8 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
         ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete;
 
         /// Compute a contact info if the two bounding volume collide
-        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+        void testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
-                                   const CollisionShapeInfo& shape2Info,
+                                   NarrowPhaseCallback* narrowPhaseCallback);
-                                   NarrowPhaseCallback* narrowPhaseCallback) override;
 };
 
 // Add a triangle vertex into the set of processed triangles

src/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -29,16 +29,6 @@
 
 using namespace reactphysics3d;
 
-/// Initialize the collision dispatch configuration
-void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
-                                    PoolAllocator* memoryAllocator) {
-
-    // Initialize the collision algorithms
-    mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
-    mGJKAlgorithm.init(collisionDetection, memoryAllocator);
-    mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
-}
-
 // Select and return the narrow-phase collision detection algorithm to
 // use between two types of collision shapes.
 NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
@@ -46,19 +36,14 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
     CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
     CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
 
-    // Sphere vs Sphere algorithm
-    if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
-        return &mSphereVsSphereAlgorithm;
-    }
-    // Concave vs Convex algorithm
-    else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
-             (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
-        return &mConcaveVsConvexAlgorithm;
-    }
     // Convex vs Convex algorithm (GJK algorithm)
-    else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
+    if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
         return &mGJKAlgorithm;
     }
+    // Sphere vs Sphere algorithm
+    else if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
+        return &mSphereVsSphereAlgorithm;
+    }
     else {
         return nullptr;
     }

src/collision/narrowphase/DefaultCollisionDispatch.h

@@ -47,9 +47,6 @@ class DefaultCollisionDispatch : public CollisionDispatch {
         /// Sphere vs Sphere collision algorithm
         SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
 
-        /// Concave vs Convex collision algorithm
-        ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
-
         /// GJK Algorithm
         GJKAlgorithm mGJKAlgorithm;
 
@@ -61,10 +58,6 @@ class DefaultCollisionDispatch : public CollisionDispatch {
         /// Destructor
         virtual ~DefaultCollisionDispatch() override = default;
 
-        /// Initialize the collision dispatch configuration
-        virtual void init(CollisionDetection* collisionDetection,
-                          PoolAllocator* memoryAllocator) override;
-
         /// Select and return the narrow-phase collision detection algorithm to
         /// use between two types of collision shapes.
         virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override;

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -74,25 +74,22 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
 /// the correct penetration depth. This method returns true if the EPA penetration
 /// depth computation has succeeded and false it has failed.
 bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
-                                                           CollisionShapeInfo shape1Info,
+                                                           const NarrowPhaseInfo* narrowPhaseInfo,
-                                                           const Transform& transform1,
-                                                           CollisionShapeInfo shape2Info,
-                                                           const Transform& transform2,
                                                            Vector3& v,
-                                                           NarrowPhaseCallback* narrowPhaseCallback) {
+                                                           ContactPointInfo& contactPointInfo) {
 
     PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
 
     decimal gjkPenDepthSquare = v.lengthSquare();
 
-    assert(shape1Info.collisionShape->isConvex());
+    assert(narrowPhaseInfo->collisionShape1->isConvex());
-    assert(shape2Info.collisionShape->isConvex());
+    assert(narrowPhaseInfo->collisionShape2->isConvex());
 
-    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
-    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
 
-    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+    void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
-    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+    void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
 
     Vector3 suppPointsA[MAX_SUPPORT_POINTS];  // Support points of object A in local coordinates
     Vector3 suppPointsB[MAX_SUPPORT_POINTS];  // Support points of object B in local coordinates
@@ -103,12 +100,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
 
     // Transform a point from local space of body 2 to local
     // space of body 1 (the GJK algorithm is done in local space of body 1)
-    Transform body2Tobody1 = transform1.getInverse() * transform2;
+    Transform body2Tobody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform;
 
     // Matrix that transform a direction from local
     // space of body 1 into local space of body 2
-    Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
+    Quaternion rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getInverse() *
-                              transform1.getOrientation();
+                              narrowPhaseInfo->shape1ToWorldTransform.getOrientation();
 
     // Get the simplex computed previously by the GJK algorithm
     int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
@@ -130,7 +127,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
             // Only one point in the simplex (which should be the origin).
             // We have a touching contact with zero penetration depth.
             // We drop that kind of contact. Therefore, we return false
-            return true;
+            return false;
 
         case 2: {
             // The simplex returned by GJK is a line segment d containing the origin.
@@ -416,7 +413,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
     } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
 
     // Compute the contact info
-    v = transform1.getOrientation() * triangle->getClosestPoint();
+    v = narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * triangle->getClosestPoint();
     Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
     Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
     Vector3 normal = v.getUnit();
@@ -430,10 +427,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
     if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) {
 
         // Create the contact info object
-        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+        contactPointInfo.init(normal, penetrationDepth, pALocal, pBLocal);
-                                     shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
-
-        narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
 
         return true;
     }

src/collision/narrowphase/EPA/EPAAlgorithm.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "collision/narrowphase/GJK/VoronoiSimplex.h"
 #include "collision/shapes/CollisionShape.h"
-#include "collision/CollisionShapeInfo.h"
+#include "collision/NarrowPhaseInfo.h"
 #include "constraint/ContactPoint.h"
 #include "collision/narrowphase/NarrowPhaseAlgorithm.h"
 #include "mathematics/mathematics.h"
@@ -87,9 +87,6 @@ class EPAAlgorithm {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to the memory allocator
-        PoolAllocator* mMemoryAllocator;
-
         /// Triangle comparison operator
         TriangleComparison mTriangleComparison;
         
@@ -119,17 +116,11 @@ class EPAAlgorithm {
         /// Deleted assignment operator
         EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete;
 
-        /// Initalize the algorithm
-        void init(PoolAllocator* memoryAllocator);
-
         /// Compute the penetration depth with EPA algorithm.
         bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
-                                                     CollisionShapeInfo shape1Info,
+                                                     const NarrowPhaseInfo* narrowPhaseInfo,
-                                                     const Transform& transform1,
-                                                     CollisionShapeInfo shape2Info,
-                                                     const Transform& transform2,
                                                      Vector3& v,
-                                                     NarrowPhaseCallback* narrowPhaseCallback);
+                                                     ContactPointInfo &contactPointInfo);
 };
 
 // Add a triangle face in the candidate triangle heap in the EPA algorithm
@@ -150,11 +141,6 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA**
     }
 }
 
-// Initalize the algorithm
-inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) {
-    mMemoryAllocator = memoryAllocator;
-}
-
 }
 
 #endif

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -36,11 +36,6 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
-// Constructor
-GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
-
-}
-
 // Compute a contact info if the two collision shapes collide.
 /// This method implements the Hybrid Technique for computing the penetration depth by
 /// running the GJK algorithm on original objects (without margin). If the shapes intersect
@@ -50,9 +45,8 @@ GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
 /// algorithm on the enlarged object to obtain a simplex polytope that contains the
 /// origin, they we give that simplex polytope to the EPA algorithm which will compute
 /// the correct penetration depth and contact points between the enlarged objects.
-void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
-                                 const CollisionShapeInfo& shape2Info,
+                                 ContactPointInfo& contactPointInfo) {
-                                 NarrowPhaseCallback* narrowPhaseCallback) {
 
     PROFILE("GJKAlgorithm::testCollision()");
     
@@ -65,20 +59,20 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
     decimal prevDistSquare;
     bool contactFound = false;
 
-    assert(shape1Info.collisionShape->isConvex());
+    assert(narrowPhaseInfo->collisionShape1->isConvex());
-    assert(shape2Info.collisionShape->isConvex());
+    assert(narrowPhaseInfo->collisionShape2->isConvex());
 
-    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
-    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
 
-    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+    void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
-    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+    void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
 
     bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
 
     // Get the local-space to world-space transforms
-    const Transform transform1 = shape1Info.shapeToWorldTransform;
+    const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
-    const Transform transform2 = shape2Info.shapeToWorldTransform;
+    const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
 
     // Transform a point from local space of body 2 to local
     // space of body 1 (the GJK algorithm is done in local space of body 1)
@@ -92,13 +86,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
     // Initialize the margin (sum of margins of both objects)
     decimal margin = shape1->getMargin() + shape2->getMargin();
     decimal marginSquare = margin * margin;
-    assert(margin > 0.0);
+    assert(margin > decimal(0.0));
 
     // Create a simplex set
     VoronoiSimplex simplex;
 
     // Get the previous point V (last cached separating axis)
-    Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
+    Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis();
 
     // Initialize the upper bound for the square distance
     decimal distSquare = DECIMAL_LARGEST;
@@ -116,13 +110,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
         vDotw = v.dot(w);
         
         // If the enlarge objects (with margins) do not intersect
-        if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
+        if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
                         
             // Cache the current separating axis for frame coherence
-            mCurrentOverlappingPair->setCachedSeparatingAxis(v);
+            narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v);
             
             // No intersection, we return
-            return;
+            return false;
         }
 
         // If the objects intersect only in the margins
@@ -179,8 +173,6 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
     } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
             distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
 
-    bool isEPAResultValid = false;
-
     // If no contact has been found (penetration case)
     if (!contactFound) {
 
@@ -188,11 +180,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
         // again but on the enlarged objects to compute a simplex polytope that contains
         // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
         // the correct penetration depth and contact points between the enlarged objects.
-        isEPAResultValid = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
+        if(computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v)) {
-                                                                     transform2, narrowPhaseCallback, v);
+
+            // A contact has been found with EPA algorithm, we return true
+            return true;
+        }
     }
 
-    if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) {
+    if (contactFound && distSquare > MACHINE_EPSILON) {
 
         // Compute the closet points of both objects (without the margins)
         simplex.computeClosestPointsOfAandB(pA, pB);
@@ -200,7 +195,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
         // Project those two points on the margins to have the closest points of both
         // object with the margins
         decimal dist = std::sqrt(distSquare);
-        assert(dist > 0.0);
+        assert(dist > decimal(0.0));
         pA = (pA - (shape1->getMargin() / dist) * v);
         pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
 
@@ -209,21 +204,22 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
         decimal penetrationDepth = margin - dist;
 
         // If the penetration depth is negative (due too numerical errors), there is no contact
-        if (penetrationDepth <= 0.0) {
+        if (penetrationDepth <= decimal(0.0)) {
-            return;
+            return false;
         }
 
         // Do not generate a contact point with zero normal length
         if (normal.lengthSquare() < MACHINE_EPSILON) {
-            return;
+            return false;
         }
 
         // Create the contact info object
-        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+        contactPointInfo.init(normal, penetrationDepth, pA, pB);
-                                     shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
 
-        narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+        return true;
     }
+
+    return false;
 }
 
 /// This method runs the GJK algorithm on the two enlarged objects (with margin)
@@ -231,11 +227,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
 /// assumed to intersect in the original objects (without margin). Therefore such
 /// a polytope must exist. Then, we give that polytope to the EPA algorithm to
 /// compute the correct penetration depth and contact points of the enlarged objects.
-bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo,
-                                                             const Transform& transform1,
+                                                             ContactPointInfo& contactPointInfo,
-                                                             const CollisionShapeInfo& shape2Info,
-                                                             const Transform& transform2,
-                                                             NarrowPhaseCallback* narrowPhaseCallback,
                                                              Vector3& v) {
     PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
 
@@ -247,24 +240,24 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
     decimal distSquare = DECIMAL_LARGEST;
     decimal prevDistSquare;
 
-    assert(shape1Info.collisionShape->isConvex());
+    assert(narrowPhaseInfo->collisionShape1->isConvex());
-    assert(shape2Info.collisionShape->isConvex());
+    assert(narrowPhaseInfo->collisionShape2->isConvex());
 
-    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
-    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
 
     bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
 
-    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+    void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
-    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+    void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
 
     // Transform a point from local space of body 2 to local space
     // of body 1 (the GJK algorithm is done in local space of body 1)
-    Transform body2ToBody1 = transform1.getInverse() * transform2;
+    Transform body2ToBody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform;
 
     // Matrix that transform a direction from local space of body 1 into local space of body 2
-    Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
+    Matrix3x3 rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getMatrix().getTranspose() *
-                              transform1.getOrientation().getMatrix();
+                              narrowPhaseInfo->shape1ToWorldTransform.getOrientation().getMatrix();
     
     do {
         // Compute the support points for the enlarged object A and B
@@ -277,7 +270,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
         vDotw = v.dot(w);
 
         // If the enlarge objects do not intersect
-        if (vDotw > 0.0) {
+        if (vDotw > decimal(0.0)) {
 
             // No intersection, we return
             return false;
@@ -308,9 +301,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
     // Give the simplex computed with GJK algorithm to the EPA algorithm
     // which will compute the correct penetration depth and contact points
     // between the two enlarged objects
-    return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
+    return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, narrowPhaseInfo, v, contactPointInfo);
-                                                            transform1, shape2Info, transform2,
-                                                            v, narrowPhaseCallback);
 }
 
 // Use the GJK Algorithm to find if a point is inside a convex collision shape
@@ -378,7 +369,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
     return true;
 }
 
-
 // Ray casting algorithm agains a convex collision shape using the GJK Algorithm
 /// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
 /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -69,11 +69,8 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         // -------------------- Methods -------------------- //
 
         /// Compute the penetration depth for enlarged objects.
-        bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
+        bool computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo,
-                                                       const Transform& transform1,
+                                                       ContactPointInfo& contactPointInfo,
-                                                       const CollisionShapeInfo& shape2Info,
-                                                       const Transform& transform2,
-                                                       NarrowPhaseCallback* narrowPhaseCallback,
                                                        Vector3& v);
 
     public :
@@ -81,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        GJKAlgorithm();
+        GJKAlgorithm() = default;
 
         /// Destructor
         ~GJKAlgorithm() = default;
@@ -92,14 +89,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
 
-        /// Initalize the algorithm
-        virtual void init(CollisionDetection* collisionDetection,
-                          PoolAllocator* memoryAllocator) override;
-
         /// Compute a contact info if the two bounding volumes collide.
-        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
-                                   const CollisionShapeInfo& shape2Info,
+                                   ContactPointInfo& contactPointInfo) override;
-                                   NarrowPhaseCallback* narrowPhaseCallback) override;
 
         /// Use the GJK Algorithm to find if a point is inside a convex collision shape
         bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
@@ -108,13 +100,6 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
 };
 
-// Initalize the algorithm
-inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
-                               PoolAllocator* memoryAllocator) {
-    NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
-    mAlgoEPA.init(memoryAllocator);
-}
-
 }
 
 #endif

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -31,7 +31,7 @@
 #include "constraint/ContactPoint.h"
 #include "memory/PoolAllocator.h"
 #include "engine/OverlappingPair.h"
-#include "collision/CollisionShapeInfo.h"
+#include "collision/NarrowPhaseInfo.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -67,21 +67,12 @@ class NarrowPhaseAlgorithm {
 
         // -------------------- Attributes -------------------- //
 
-        /// Pointer to the collision detection object
-        CollisionDetection* mCollisionDetection;
-
-        /// Pointer to the memory allocator
-        PoolAllocator* mMemoryAllocator;
-
-        /// Overlapping pair of the bodies currently tested for collision
-        OverlappingPair* mCurrentOverlappingPair;
-
     public :
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        NarrowPhaseAlgorithm();
+        NarrowPhaseAlgorithm() = default;
 
         /// Destructor
         virtual ~NarrowPhaseAlgorithm() = default;
@@ -92,23 +83,10 @@ class NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
 
-        /// Initalize the algorithm
-        virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator);
-        
-        /// Set the current overlapping pair of bodies
-        void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
-
         /// Compute a contact info if the two bounding volume collide
-        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0;
-                                   const CollisionShapeInfo& shape2Info,
-                                   NarrowPhaseCallback* narrowPhaseCallback)=0;
 };
 
-// Set the current overlapping pair of bodies
-inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
-    mCurrentOverlappingPair = overlappingPair;
-}      
-
 }
 
 #endif

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -30,17 +30,16 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
 
-void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
-                                            const CollisionShapeInfo& shape2Info,
+                                            ContactPointInfo& contactPointInfo) {
-                                            NarrowPhaseCallback* narrowPhaseCallback) {
     
     // Get the sphere collision shapes
-    const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
+    const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
-    const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
+    const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
 
     // Get the local-space to world-space transforms
-    const Transform& transform1 = shape1Info.shapeToWorldTransform;
+    const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
-    const Transform& transform2 = shape2Info.shapeToWorldTransform;
+    const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
 
     // Compute the distance between the centers
     Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@@ -60,11 +59,11 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info
         decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
         
         // Create the contact info object
-        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+        contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth,
-                                     shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
                               intersectionOnBody1, intersectionOnBody2);
 
-        // Notify about the new contact
+        return true;
-        narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
     }
+
+    return false;
 }

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -61,9 +61,8 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
         SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
 
         /// Compute a contact info if the two bounding volume collide
-        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
-                                   const CollisionShapeInfo& shape2Info,
+                                   ContactPointInfo& contactPointInfo) override;
-                                   NarrowPhaseCallback* narrowPhaseCallback) override;
 };
 
 }

src/constraint/ContactPoint.cpp

@@ -32,17 +32,10 @@ using namespace std;
 
 // Constructor
 ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
-             : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
+             : mNormal(contactInfo.normal),
-               mNormal(contactInfo.normal),
                mPenetrationDepth(contactInfo.penetrationDepth),
                mLocalPointOnBody1(contactInfo.localPoint1),
                mLocalPointOnBody2(contactInfo.localPoint2),
-               mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
-                                  contactInfo.shape1->getLocalToBodyTransform() *
-                                  contactInfo.localPoint1),
-               mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
-                                  contactInfo.shape2->getLocalToBodyTransform() *
-                                  contactInfo.localPoint2),
                mIsRestingContact(false) {
 
     assert(mPenetrationDepth > decimal(0.0));

src/constraint/ContactPoint.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "body/CollisionBody.h"
-#include "collision/CollisionShapeInfo.h"
+#include "collision/NarrowPhaseInfo.h"
 #include "configuration.h"
 #include "mathematics/mathematics.h"
 #include "configuration.h"
@@ -53,18 +53,6 @@ struct ContactPointInfo {
 
         // -------------------- Attributes -------------------- //
 
-        /// First proxy shape of the contact
-        ProxyShape* shape1;
-
-        /// Second proxy shape of the contact
-        ProxyShape* shape2;
-
-        /// First collision shape
-        const CollisionShape* collisionShape1;
-
-        /// Second collision shape
-        const CollisionShape* collisionShape2;
-
         /// Normalized normal vector of the collision contact in world space
         Vector3 normal;
 
@@ -80,13 +68,18 @@ struct ContactPointInfo {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
+        ContactPointInfo() = default;
-                         const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth,
-                         const Vector3& localPoint1, const Vector3& localPoint2)
-            : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
-              normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
-              localPoint2(localPoint2) {
 
+        /// Destructor
+        ~ContactPointInfo() = default;
+
+        /// Initialize the contact point info
+        void init(const Vector3& contactNormal, decimal penDepth,
+                  const Vector3& localPt1, const Vector3& localPt2) {
+            normal = contactNormal;
+            penetrationDepth = penDepth;
+            localPoint1 = localPt1;
+            localPoint2 = localPt2;
         }
 };
 
@@ -101,12 +94,6 @@ class ContactPoint {
 
         // -------------------- Attributes -------------------- //
 
-        /// First rigid body of the contact
-        CollisionBody* mBody1;
-
-        /// Second rigid body of the contact
-        CollisionBody* mBody2;
-
         /// Normalized normal vector of the contact (from body1 toward body2) in world space
         const Vector3 mNormal;
 
@@ -147,18 +134,15 @@ class ContactPoint {
         /// Deleted assignment operator
         ContactPoint& operator=(const ContactPoint& contact) = delete;
 
-        /// Return the reference to the body 1
+        /// Update the world contact points
-        CollisionBody* getBody1() const;
+        void updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform);
 
-        /// Return the reference to the body 2
+        /// Update the penetration depth
-        CollisionBody* getBody2() const;
+        void updatePenetrationDepth();
 
         /// Return the normal vector of the contact
         Vector3 getNormal() const;
 
-        /// Set the penetration depth of the contact
-        void setPenetrationDepth(decimal penetrationDepth);
-
         /// Return the contact local point on body 1
         Vector3 getLocalPointOnBody1() const;
 
@@ -177,12 +161,6 @@ class ContactPoint {
         /// Set the cached penetration impulse
         void setPenetrationImpulse(decimal impulse);
 
-        /// Set the contact world point on body 1
-        void setWorldPointOnBody1(const Vector3& worldPoint);
-
-        /// Set the contact world point on body 2
-        void setWorldPointOnBody2(const Vector3& worldPoint);
-
         /// Return true if the contact is a resting contact
         bool getIsRestingContact() const;
 
@@ -196,14 +174,15 @@ class ContactPoint {
         size_t getSizeInBytes() const;
 };
 
-// Return the reference to the body 1
+// Update the world contact points
-inline CollisionBody* ContactPoint::getBody1() const {
+inline void ContactPoint::updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform) {
-    return mBody1;
+    mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
+    mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
 }
 
-// Return the reference to the body 2
+// Update the penetration depth
-inline CollisionBody* ContactPoint::getBody2() const {
+inline void ContactPoint::updatePenetrationDepth() {
-    return mBody2;
+    mPenetrationDepth = (mWorldPointOnBody1 - mWorldPointOnBody2).dot(mNormal);
 }
 
 // Return the normal vector of the contact
@@ -211,11 +190,6 @@ inline Vector3 ContactPoint::getNormal() const {
     return mNormal;
 }
 
-// Set the penetration depth of the contact
-inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) {
-    this->mPenetrationDepth = penetrationDepth;
-}
-
 // Return the contact point on body 1
 inline Vector3 ContactPoint::getLocalPointOnBody1() const {
     return mLocalPointOnBody1;
@@ -246,16 +220,6 @@ inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
     mPenetrationImpulse = impulse;
 }
 
-// Set the contact world point on body 1
-inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
-    mWorldPointOnBody1 = worldPoint;
-}
-
-// Set the contact world point on body 2
-inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
-    mWorldPointOnBody2 = worldPoint;
-}
-
 // Return true if the contact is a resting contact
 inline bool ContactPoint::getIsRestingContact() const {
     return mIsRestingContact;

src/containers/LinkedList.h

@@ -0,0 +1,124 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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_LINKED_LIST_H
+#define REACTPHYSICS3D_LINKED_LIST_H
+
+// Libraries
+#include "memory/Allocator.h"
+
+namespace reactphysics3d {
+
+// Class LinkedList
+/**
+ * This class represents a simple generic linked list.
+  */
+template<typename T>
+class LinkedList {
+
+    public:
+
+        /// Element of the linked list
+        struct ListElement {
+
+            /// Data of the list element
+            T data;
+
+            /// Pointer to the next element of the list
+            ListElement* next;
+
+            /// Constructor
+            ListElement(T elementData, ListElement* nextElement)
+                : data(elementData), next(nextElement) {
+
+            }
+        };
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the first element of the list
+        ListElement* mListHead;
+
+        /// Memory allocator used to allocate the list elements
+        Allocator& mAllocator;
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        LinkedList(Allocator& allocator) : mListHead(nullptr), mAllocator(allocator) {
+
+        }
+
+        /// Destructor
+        ~LinkedList() {
+            reset();
+        }
+
+        /// Return the first element of the list
+        ListElement* getListHead() const;
+
+        /// Insert an element at the beginning of the linked list
+        void insert(const T& data);
+
+        /// Remove all the elements of the list
+        void reset();
+
+};
+
+// Return the first element of the list
+template<typename T>
+inline typename LinkedList<T>::ListElement* LinkedList<T>::getListHead() const {
+    return mListHead;
+}
+
+// Insert an element at the beginning of the linked list
+template<typename T>
+inline void LinkedList<T>::insert(const T& data) {
+    ListElement* element = new (mAllocator.allocate(sizeof(ListElement))) ListElement(data, mListHead);
+    mListHead = element;
+}
+
+// Remove all the elements of the list
+template<typename T>
+inline void LinkedList<T>::reset() {
+
+    // Release all the list elements
+    ListElement* element = mListHead;
+    while (element != nullptr) {
+        ListElement* nextElement = element->next;
+        mAllocator.release(element, sizeof(ListElement));
+        element = nextElement;
+    }
+
+    mListHead = nullptr;
+}
+
+}
+
+#endif

src/engine/CollisionWorld.cpp

@@ -33,7 +33,8 @@ using namespace std;
 
 // Constructor
 CollisionWorld::CollisionWorld()
-               : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0),
+               : mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES),
+                 mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0),
                  mEventListener(nullptr) {
 
 }
@@ -148,119 +149,18 @@ bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
     return body1AABB.testCollision(body2AABB);
 }
 
-// Test and report collisions between a given shape and all the others
+// Report all the bodies that overlap with the aabb in parameter
-// shapes of the world.
 /**
- * @param shape Pointer to the proxy shape to test
+ * @param aabb AABB used to test for overlap
- * @param callback Pointer to the object with the callback method
+ * @param overlapCallback Pointer to the callback class to report overlap
+ * @param categoryMaskBits bits mask used to filter the bodies to test overlap with
  */
-void CollisionWorld::testCollision(const ProxyShape* shape,
+inline void CollisionWorld::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
-                                   CollisionCallback* callback) {
+    mCollisionDetection.testAABBOverlap(aabb, overlapCallback, categoryMaskBits);
-
-    // Reset all the contact manifolds lists of each body
-    resetContactManifoldListsOfBodies();
-
-    // Create the sets of shapes
-    std::set<uint> shapes;
-    shapes.insert(shape->mBroadPhaseID);
-    std::set<uint> emptySet;
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
 }
 
-// Test and report collisions between two given shapes
+// Return true if two bodies overlap
-/**
+bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) {
- * @param shape1 Pointer to the first proxy shape to test
+    return mCollisionDetection.testOverlap(body1, body2);
- * @param shape2 Pointer to the second proxy shape to test
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(const ProxyShape* shape1,
-                                   const ProxyShape* shape2,
-                                   CollisionCallback* callback) {
-
-    // Reset all the contact manifolds lists of each body
-    resetContactManifoldListsOfBodies();
-
-    // Create the sets of shapes
-    std::set<uint> shapes1;
-    shapes1.insert(shape1->mBroadPhaseID);
-    std::set<uint> shapes2;
-    shapes2.insert(shape2->mBroadPhaseID);
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between a body and all the others bodies of the
-// world
-/**
- * @param body Pointer to the first body to test
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(const CollisionBody* body,
-                                   CollisionCallback* callback) {
-
-    // Reset all the contact manifolds lists of each body
-    resetContactManifoldListsOfBodies();
-
-    // Create the sets of shapes
-    std::set<uint> shapes1;
-
-    // For each shape of the body
-    for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr;
-         shape = shape->getNext()) {
-        shapes1.insert(shape->mBroadPhaseID);
-    }
-
-    std::set<uint> emptySet;
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
-}
-
-// Test and report collisions between two bodies
-/**
- * @param body1 Pointer to the first body to test
- * @param body2 Pointer to the second body to test
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(const CollisionBody* body1,
-                                   const CollisionBody* body2,
-                                   CollisionCallback* callback) {
-
-    // Reset all the contact manifolds lists of each body
-    resetContactManifoldListsOfBodies();
-
-    // Create the sets of shapes
-    std::set<uint> shapes1;
-    for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
-         shape = shape->getNext()) {
-        shapes1.insert(shape->mBroadPhaseID);
-    }
-
-    std::set<uint> shapes2;
-    for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr;
-         shape = shape->getNext()) {
-        shapes2.insert(shape->mBroadPhaseID);
-    }
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between all shapes of the world
-/**
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(CollisionCallback* callback) {
-
-    // Reset all the contact manifolds lists of each body
-    resetContactManifoldListsOfBodies();
-
-    std::set<uint> emptySet;
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
 }
 

src/engine/CollisionWorld.h

@@ -47,6 +47,7 @@ namespace reactphysics3d {
 
 // Declarations
 class CollisionCallback;
+class OverlapCallback;
 
 // Class CollisionWorld
 /**
@@ -60,6 +61,12 @@ class CollisionWorld {
 
         // -------------------- Attributes -------------------- //
 
+        /// Pool Memory allocator
+        PoolAllocator mPoolAllocator;
+
+        /// Single frame Memory allocator
+        SingleFrameAllocator mSingleFrameAllocator;
+
         /// Reference to the collision detection
         CollisionDetection mCollisionDetection;
 
@@ -72,9 +79,6 @@ class CollisionWorld {
         /// List of free ID for rigid bodies
         std::vector<luint> mFreeBodiesIDs;
 
-        /// Pool Memory allocator
-        PoolAllocator mPoolAllocator;
-
         /// Pointer to an event listener object
         EventListener* mEventListener;
 
@@ -125,32 +129,23 @@ class CollisionWorld {
         bool testAABBOverlap(const CollisionBody* body1,
                              const CollisionBody* body2) const;
 
-        /// Test if the AABBs of two proxy shapes overlap
+        /// Report all the bodies that overlap with the AABB in parameter
-        bool testAABBOverlap(const ProxyShape* shape1,
+        void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-                             const ProxyShape* shape2) const;
 
-        /// Test and report collisions between a given shape and all the others
+        /// Return true if two bodies overlap
-        /// shapes of the world
+        bool testOverlap(CollisionBody* body1, CollisionBody* body2);
-        virtual void testCollision(const ProxyShape* shape,
-                                   CollisionCallback* callback);
 
-        /// Test and report collisions between two given shapes
+        /// Report all the bodies that overlap with the body in parameter
-        virtual void testCollision(const ProxyShape* shape1,
+        void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-                                   const ProxyShape* shape2,
-                                   CollisionCallback* callback);
-
-        /// Test and report collisions between a body and all the others bodies of the
-        /// world
-        virtual void testCollision(const CollisionBody* body,
-                                   CollisionCallback* callback);
 
         /// Test and report collisions between two bodies
-        virtual void testCollision(const CollisionBody* body1,
+        void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
-                                   const CollisionBody* body2,
+
-                                   CollisionCallback* callback);
+        /// Test and report collisions between a body and all the others bodies of the world
+        void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
 
         /// Test and report collisions between all shapes of the world
-        virtual void testCollision(CollisionCallback* callback);
+        void testCollision(CollisionCallback* callback);
 
         // -------------------- Friendship -------------------- //
 
@@ -200,36 +195,100 @@ inline void CollisionWorld::raycast(const Ray& ray,
     mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
 }
 
-// Test if the AABBs of two proxy shapes overlap
+// Test and report collisions between two bodies
 /**
- * @param shape1 Pointer to the first proxy shape to test
+ * @param body1 Pointer to the first body to test
- * @param shape2 Pointer to the second proxy shape to test
+ * @param body2 Pointer to the second body to test
- * @return
+ * @param callback Pointer to the object with the callback method
  */
-inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
+inline void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback) {
-                                            const ProxyShape* shape2) const {
+    mCollisionDetection.testCollision(body1, body2, callback);
+}
 
-    return mCollisionDetection.testAABBOverlap(shape1, shape2);
+// Test and report collisions between a body and all the others bodies of the world
+/**
+ * @param body Pointer to the body against which we need to test collision
+ * @param callback Pointer to the object with the callback method to report contacts
+ * @param categoryMaskBits Bits mask corresponding to the category of bodies we need to test collision with
+ */
+inline void CollisionWorld::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) {
+    mCollisionDetection.testCollision(body, callback, categoryMaskBits);
+}
+
+// Test and report collisions between all bodies of the world
+/**
+ * @param callback Pointer to the object with the callback method to report contacts
+ */
+inline void CollisionWorld::testCollision(CollisionCallback* callback) {
+    mCollisionDetection.testCollision(callback);
+}
+
+// Report all the bodies that overlap with the body in parameter
+/**
+ * @param body Pointer to the collision body to test overlap with
+ * @param overlapCallback Pointer to the callback class to report overlap
+ * @param categoryMaskBits bits mask used to filter the bodies to test overlap with
+ */
+inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
+    mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
 }
 
 // Class CollisionCallback
 /**
  * This class can be used to register a callback for collision test queries.
  * You should implement your own class inherited from this one and implement
- * the notifyRaycastHit() method. This method will be called for each ProxyShape
+ * the notifyContact() method. This method will called each time a contact
- * that is hit by the ray.
+ * point is reported.
  */
 class CollisionCallback {
 
     public:
 
+        struct CollisionCallbackInfo {
+
+            public:
+                const ContactPointInfo& contactPoint;
+                CollisionBody* body1;
+                CollisionBody* body2;
+                const ProxyShape* proxyShape1;
+                const ProxyShape* proxyShape2;
+
+                // Constructor
+                CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2,
+                                      const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
+                    contactPoint(point), body1(b1), body2(b2),
+                    proxyShape1(proxShape1), proxyShape2(proxShape2) {
+
+                }
+        };
+
         /// Destructor
         virtual ~CollisionCallback() {
 
         }
 
-        /// This method will be called for contact
+        /// This method will be called for each reported contact point
-        virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0;
+        virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
+};
+
+// Class OverlapCallback
+/**
+ * This class can be used to register a callback for collision overlap queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyOverlap() method. This method will called each time a contact
+ * point is reported.
+ */
+class OverlapCallback {
+
+    public:
+
+        /// Destructor
+        virtual ~OverlapCallback() {
+
+        }
+
+        /// This method will be called for each reported overlapping bodies
+        virtual void notifyOverlap(CollisionBody* collisionBody)=0;
 };
 
 }

src/engine/ContactSolver.cpp

@@ -115,8 +115,8 @@ void ContactSolver::initializeForIsland(Island* island) {
         assert(externalManifold->getNbContactPoints() > 0);
 
         // Get the two bodies of the contact
-        RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody1());
+        RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getBody1());
-        RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody2());
+        RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getBody2());
         assert(body1 != nullptr);
         assert(body2 != nullptr);
 

src/engine/DynamicsWorld.cpp

@@ -40,7 +40,6 @@ using namespace std;
  */
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
               : CollisionWorld(),
-                mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES),
                 mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator),
                 mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
@@ -829,117 +828,6 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
     }
 }
 
-// Test and report collisions between a given shape and all the others
-// shapes of the world.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param shape Pointer to the proxy shape to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const ProxyShape* shape,
-                                   CollisionCallback* callback) {
-
-    // Create the sets of shapes
-    std::set<uint> shapes;
-    shapes.insert(shape->mBroadPhaseID);
-    std::set<uint> emptySet;
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
-}
-
-// Test and report collisions between two given shapes.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param shape1 Pointer to the first proxy shape to test
- * @param shape2 Pointer to the second proxy shape to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const ProxyShape* shape1,
-                                   const ProxyShape* shape2,
-                                   CollisionCallback* callback) {
-
-    // Create the sets of shapes
-    std::set<uint> shapes1;
-    shapes1.insert(shape1->mBroadPhaseID);
-    std::set<uint> shapes2;
-    shapes2.insert(shape2->mBroadPhaseID);
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between a body and all the others bodies of the
-// world.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param body Pointer to the first body to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const CollisionBody* body,
-                                   CollisionCallback* callback) {
-
-    // Create the sets of shapes
-    std::set<uint> shapes1;
-
-    // For each shape of the body
-    for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr;
-         shape = shape->getNext()) {
-        shapes1.insert(shape->mBroadPhaseID);
-    }
-
-    std::set<uint> emptySet;
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
-}
-
-// Test and report collisions between two bodies.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param body1 Pointer to the first body to test
- * @param body2 Pointer to the second body to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const CollisionBody* body1,
-                                   const CollisionBody* body2,
-                                   CollisionCallback* callback) {
-
-    // Create the sets of shapes
-    std::set<uint> shapes1;
-    for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
-         shape = shape->getNext()) {
-        shapes1.insert(shape->mBroadPhaseID);
-    }
-
-    std::set<uint> shapes2;
-    for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr;
-         shape = shape->getNext()) {
-        shapes2.insert(shape->mBroadPhaseID);
-    }
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between all shapes of the world.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(CollisionCallback* callback) {
-
-    std::set<uint> emptySet;
-
-    // Perform the collision detection and report contacts
-    mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
-}
-
 /// Return the list of all contacts of the world
 std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
 

src/engine/DynamicsWorld.h

@@ -50,9 +50,6 @@ class DynamicsWorld : public CollisionWorld {
 
         // -------------------- Attributes -------------------- //
 
-        /// Single frame Memory allocator
-        SingleFrameAllocator mSingleFrameAllocator;
-
         /// Contact solver
         ContactSolver mContactSolver;
 
@@ -267,29 +264,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Set an event listener object to receive events callbacks.
         void setEventListener(EventListener* eventListener);
 
-        /// Test and report collisions between a given shape and all the others
-        /// shapes of the world
-        virtual void testCollision(const ProxyShape* shape,
-                                   CollisionCallback* callback) override;
-
-        /// Test and report collisions between two given shapes
-        virtual void testCollision(const ProxyShape* shape1,
-                                   const ProxyShape* shape2,
-                                   CollisionCallback* callback) override;
-
-        /// Test and report collisions between a body and all
-        /// the others bodies of the world
-        virtual void testCollision(const CollisionBody* body,
-                                   CollisionCallback* callback) override;
-
-        /// Test and report collisions between two bodies
-        virtual void testCollision(const CollisionBody* body1,
-                                   const CollisionBody* body2,
-                                   CollisionCallback* callback) override;
-
-        /// Test and report collisions between all shapes of the world
-        virtual void testCollision(CollisionCallback* callback) override;
-
         /// Return the list of all contacts of the world
         std::vector<const ContactManifold*> getContactsList() const;
 

src/memory/Allocator.h

@@ -23,20 +23,34 @@
 *                                                                               *
 ********************************************************************************/
 
+#ifndef REACTPHYSICS3D_ALLOCATOR_H
+#define REACTPHYSICS3D_ALLOCATOR_H
+
 // Libraries
-#include "NarrowPhaseAlgorithm.h"
+#include <cstring>
 
-// We want to use the ReactPhysics3D namespace
+/// ReactPhysics3D namespace
-using namespace reactphysics3d;
+namespace reactphysics3d {
 
-// Constructor
+// Class Allocator
-NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
+/**
-                     : mMemoryAllocator(nullptr), mCurrentOverlappingPair(nullptr) {
+ * Abstract class with the basic interface of all the derived memory allocators
+ */
+class Allocator {
+
+    public:
+
+        /// Destructor
+        virtual ~Allocator() = default;
+
+        /// Allocate memory of a given size (in bytes) and return a pointer to the
+        /// allocated memory.
+        virtual void* allocate(size_t size)=0;
+
+        /// Release previously allocated memory.
+        virtual void release(void* pointer, size_t size)=0;
+};
 
 }
 
-// Initalize the algorithm
+#endif
-void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) {
-    mCollisionDetection = collisionDetection;
-    mMemoryAllocator = memoryAllocator;
-}

src/memory/PoolAllocator.h

@@ -27,8 +27,8 @@
 #define REACTPHYSICS3D_POOL_ALLOCATOR_H
 
 // Libraries
-#include <cstring>
 #include "configuration.h"
+#include "Allocator.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -40,7 +40,7 @@ namespace reactphysics3d {
  * efficiently. This implementation is inspired by the small block allocator
  * described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp
  */
-class PoolAllocator {
+class PoolAllocator : public Allocator {
 
     private :
 
@@ -129,14 +129,14 @@ class PoolAllocator {
         PoolAllocator();
 
         /// Destructor
-        ~PoolAllocator();
+        virtual ~PoolAllocator() override;
 
         /// Allocate memory of a given size (in bytes) and return a pointer to the
         /// allocated memory.
-        void* allocate(size_t size);
+        virtual void* allocate(size_t size) override;
 
         /// Release previously allocated memory.
-        void release(void* pointer, size_t size);
+        virtual void release(void* pointer, size_t size) override;
 
 };
 

src/memory/SingleFrameAllocator.h

@@ -27,7 +27,7 @@
 #define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H
 
 // Libraries
-#include <cstring>
+#include "Allocator.h"
 #include "configuration.h"
 
 /// ReactPhysics3D namespace
@@ -38,7 +38,7 @@ namespace reactphysics3d {
  * This class represent a memory allocator used to efficiently allocate
  * memory on the heap that is used during a single frame.
  */
-class SingleFrameAllocator {
+class SingleFrameAllocator : public Allocator {
 
     private :
 
@@ -61,13 +61,18 @@ class SingleFrameAllocator {
         SingleFrameAllocator(size_t totalSizeBytes);
 
         /// Destructor
-        ~SingleFrameAllocator();
+        virtual ~SingleFrameAllocator() override;
 
         /// Allocate memory of a given size (in bytes)
-        void* allocate(size_t size);
+        virtual void* allocate(size_t size) override;
+
+        /// Release previously allocated memory.
+        virtual void release(void* pointer, size_t size) override { }
 
         /// Reset the marker of the current allocated memory
-        void reset();
+        virtual void reset();
+
+
 
 };
 

test/tests/collision/TestCollisionWorld.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "reactphysics3d.h"
+#include "Test.h"
 
 /// Reactphysics3D namespace
 namespace reactphysics3d {
@@ -70,28 +71,28 @@ class WorldCollisionCallback : public CollisionCallback
         }
 
         // This method will be called for contact
-        virtual void notifyContact(const ContactPointInfo& contactPointInfo) override {
+        virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
 
-            if (isContactBetweenBodies(boxBody, sphere1Body, contactPointInfo)) {
+            if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) {
                 boxCollideWithSphere1 = true;
             }
-            else if (isContactBetweenBodies(boxBody, cylinderBody, contactPointInfo)) {
+            else if (isContactBetweenBodies(boxBody, cylinderBody, collisionCallbackInfo)) {
                 boxCollideWithCylinder = true;
             }
-            else if (isContactBetweenBodies(sphere1Body, cylinderBody, contactPointInfo)) {
+            else if (isContactBetweenBodies(sphere1Body, cylinderBody, collisionCallbackInfo)) {
                 sphere1CollideWithCylinder = true;
             }
-            else if (isContactBetweenBodies(sphere1Body, sphere2Body, contactPointInfo)) {
+            else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) {
                 sphere1CollideWithSphere2 = true;
             }
         }
 
         bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2,
-                                     const ContactPointInfo& contactPointInfo) {
+                                    const CollisionCallbackInfo& collisionCallbackInfo) {
-            return (contactPointInfo.shape1->getBody()->getID() == body1->getID() &&
+            return (collisionCallbackInfo.body1->getID() == body1->getID() &&
-                    contactPointInfo.shape2->getBody()->getID() == body2->getID()) ||
+                    collisionCallbackInfo.body2->getID() == body2->getID()) ||
-                   (contactPointInfo.shape2->getBody()->getID() == body1->getID() &&
+                   (collisionCallbackInfo.body2->getID() == body1->getID() &&
-                    contactPointInfo.shape1->getBody()->getID() == body2->getID());
+                    collisionCallbackInfo.body1->getID() == body2->getID());
         }
 };
 
@@ -197,10 +198,10 @@ class TestCollisionWorld : public Test {
             test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
             test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
 
-            test(mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape));
+            test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
-            test(mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape));
+            test(mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
-            test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape));
+            test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
-            test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape));
+            test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
 
             mCollisionCallback.reset();
             mWorld->testCollision(mCylinderBody, &mCollisionCallback);
@@ -223,20 +224,6 @@ class TestCollisionWorld : public Test {
             test(!mCollisionCallback.sphere1CollideWithCylinder);
             test(!mCollisionCallback.sphere1CollideWithSphere2);
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(mCylinderProxyShape, &mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(mCollisionCallback.boxCollideWithCylinder);
-            test(!mCollisionCallback.sphere1CollideWithCylinder);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
-
-            mCollisionCallback.reset();
-            mWorld->testCollision(mBoxProxyShape, mCylinderProxyShape, &mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(mCollisionCallback.boxCollideWithCylinder);
-            test(!mCollisionCallback.sphere1CollideWithCylinder);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
-
             // Move sphere 1 to collide with sphere 2
             mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
 
@@ -282,10 +269,10 @@ class TestCollisionWorld : public Test {
             test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
             test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
 
-            test(!mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape));
+            test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
-            test(!mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape));
+            test(!mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
-            test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape));
+            test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
-            test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape));
+            test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
 
             mBoxBody->setIsActive(true);
             mCylinderBody->setIsActive(true);

test/tests/collision/TestDynamicAABBTree.h

@@ -34,7 +34,7 @@
 /// Reactphysics3D namespace
 namespace reactphysics3d {
 
-class OverlapCallback : public DynamicAABBTreeOverlapCallback {
+class TestOverlapCallback : public DynamicAABBTreeOverlapCallback {
 
     public :
 
@@ -86,11 +86,9 @@ class TestDynamicAABBTree : public Test {
 
         // ---------- Atributes ---------- //
 
-        OverlapCallback mOverlapCallback;
+        TestOverlapCallback mOverlapCallback;
         DynamicTreeRaycastCallback mRaycastCallback;
 
-
-
     public :
 
         // ---------- Methods ---------- //