Working on contacts refactoring

src/collision/CollisionDetection.cpp

@@ -43,6 +43,7 @@
 #include "engine/EventListener.h"
 #include "collision/RaycastInfo.h"
 #include <cassert>
+#include <iostream>
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -400,6 +401,9 @@ void CollisionDetection::processAllPotentialContacts(NarrowPhaseInput& narrowPha
 // Compute the narrow-phase collision detection
 void CollisionDetection::computeNarrowPhase() {
 
+    // TODO : DELETE THIS
+    //std::cout << "---------------------- NARROW PHASE ----------------------------------------" << std::endl;
+
     RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler);
 
     MemoryAllocator& allocator = mMemoryManager.getSingleFrameAllocator();
@@ -617,6 +621,46 @@ void CollisionDetection::initContactsWithPreviousOnes() {
     mPreviousContactManifolds->clear();
     mPreviousContactPairs->clear();
     mPreviousMapPairIdToContactPairIndex->clear();
+
+    /*
+    // TODO : DELETE THIS
+    std::cout << "_______________ RECAP ACTUAL CONTACTS___________________" << std::endl;
+    std::cout << "ContactPairs :" << std::endl;
+    for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+
+        ContactPair& pair = (*mCurrentContactPairs)[i];
+        std::cout << "  PairId : (" << pair.pairId.first << ", " << pair.pairId.second << std::endl;
+        std::cout << "  Index : " << i << std::endl;
+        std::cout << "  ContactManifoldsIndex : " << pair.contactManifoldsIndex << std::endl;
+        std::cout << "  nbManifolds : " << pair.nbContactManifolds << std::endl;
+        std::cout << "  ContactPointsIndex : " << pair.contactPointsIndex << std::endl;
+        std::cout << "  nbTotalPoints : " << pair.nbToTalContactPoints << std::endl;
+
+    }
+    std::cout << "ContactManifolds :" << std::endl;
+    for (uint i=0; i < mCurrentContactManifolds->size(); i++) {
+
+        ContactManifold& manifold = (*mCurrentContactManifolds)[i];
+        std::cout << "  Index : " << i << std::endl;
+        std::cout << "  >>ContactPointsIndex : " << manifold.mContactPointsIndex << std::endl;
+        std::cout << "  >>Nb Contact Points : " << manifold.mNbContactPoints << std::endl;
+    }
+    std::cout << "ContactPoints :" << std::endl;
+    for (uint i=0; i < mCurrentContactPoints->size(); i++) {
+
+        ContactPoint& contactPoint = (*mCurrentContactPoints)[i];
+        std::cout << "  Index : " << i << std::endl;
+        std::cout << "  Point : (" << contactPoint.getLocalPointOnShape1().x << ", " << contactPoint.getLocalPointOnShape1().y << ", " << contactPoint.getLocalPointOnShape1().z << std::endl;
+    }
+    std::cout << "mCurrentMapPairIdToContactPairIndex :" << std::endl;
+    for (auto it = mCurrentMapPairIdToContactPairIndex->begin(); it != mCurrentMapPairIdToContactPairIndex->end(); ++it) {
+
+        OverlappingPair::OverlappingPairId pairId = it->first;
+        uint index = it->second;
+        std::cout << "  PairId : " << pairId.first << ", " << pairId.second << std::endl;
+        std::cout << "  ContactPair Index : " << index << std::endl;
+    }
+    */
 }
 
 // Remove a body from the collision detection
@@ -748,7 +792,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
         // Add the potential contacts
         for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
 
-            if (narrowPhaseInfoBatch.isColliding[i]) {
+            assert(narrowPhaseInfoBatch.isColliding[i]);
 
             const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
 
@@ -760,12 +804,14 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
 
             bool similarManifoldFound = false;
 
-                // If there are already potential contact manifolds for this overlapping pair
+            // If there is already a contact pair for this overlapping pair
             OverlappingPair::OverlappingPairId pairId = narrowPhaseInfoBatch.overlappingPairs[i]->getId();
             auto it = mCurrentMapPairIdToContactPairIndex->find(pairId);
             ContactPair* pairContact = nullptr;
             if (it != mCurrentMapPairIdToContactPairIndex->end()) {
 
+                assert(it->first == pairId);
+
                 const uint pairContactIndex = it->second;
                 pairContact = &((*mCurrentContactPairs)[pairContactIndex]);
 
@@ -773,6 +819,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
 
                 // For each contact manifold of the overlapping pair
                 for (uint m=0; m < pairContact->potentialContactManifoldsIndices.size(); m++) {
+
                    uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
 
                    // Get the first contact point of the current manifold
@@ -785,7 +832,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
                     if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
 
                         // Add the contact point to the manifold
-                            mPotentialContactManifolds[m].potentialContactPointsIndices.add(contactPointIndex);
+                        mPotentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex);
 
                         similarManifoldFound = true;
 
@@ -822,12 +869,12 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
                 mPotentialContactManifolds.add(contactManifoldInfo);
 
                 // Add the contact manifold to the overlapping pair contact
+                assert(pairContact->pairId == contactManifoldInfo.pairId);
                 pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
             }
 
             assert(pairContact->potentialContactManifoldsIndices.size() > 0);
         }
-        }
 
         narrowPhaseInfoBatch.resetContactPoints(i);
     }
@@ -836,6 +883,49 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
 // Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds
 void CollisionDetection::reducePotentialContactManifolds(const OverlappingPairMap& overlappingPairs) {
 
+    /*
+    // TODO : DELETE THIS
+    std::cout << "_______________ RECAP POTENTIAL CONTACTS___________________" << std::endl;
+    std::cout << "ContactPairs :" << std::endl;
+    for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+
+        ContactPair& pair = (*mCurrentContactPairs)[i];
+        std::cout << "  PairId : (" << pair.pairId.first << ", " << pair.pairId.second << std::endl;
+        std::cout << "  Index : " << i << std::endl;
+        std::cout << "  >>Manifolds : " << std::endl;
+        for (uint j=0; j < pair.potentialContactManifoldsIndices.size(); j++) {
+
+            std::cout << "    >>Manifold Index : " << pair.potentialContactManifoldsIndices[j] << std::endl;
+        }
+    }
+    std::cout << "PotentialContactManifolds :" << std::endl;
+    for (uint i=0; i < mPotentialContactManifolds.size(); i++) {
+
+        ContactManifoldInfo& manifold = mPotentialContactManifolds[i];
+        std::cout << "  PairId : (" << manifold.pairId.first << ", " << manifold.pairId.second << std::endl;
+        std::cout << "  Index : " << i << std::endl;
+        std::cout << "  >>PotentialContactPoints : " << std::endl;
+        for (uint j=0; j < manifold.potentialContactPointsIndices.size(); j++) {
+            std::cout << "    >>Contact Point Index : " << manifold.potentialContactPointsIndices[j] << std::endl;
+        }
+    }
+    std::cout << "PotentialContactPoints :" << std::endl;
+    for (uint i=0; i < mPotentialContactPoints.size(); i++) {
+
+        ContactPointInfo& contactPoint = mPotentialContactPoints[i];
+        std::cout << "  Index : " << i << std::endl;
+        std::cout << "  Point : (" << contactPoint.localPoint1.x << ", " << contactPoint.localPoint1.y << ", " << contactPoint.localPoint1.z << std::endl;
+    }
+    std::cout << "PotentialContactPoints :" << std::endl;
+    for (auto it = mCurrentMapPairIdToContactPairIndex->begin(); it != mCurrentMapPairIdToContactPairIndex->end(); ++it) {
+
+        OverlappingPair::OverlappingPairId pairId = it->first;
+        uint index = it->second;
+        std::cout << "  PairId : " << pairId.first << ", " << pairId.second << std::endl;
+        std::cout << "  ContactPair Index : " << index << std::endl;
+    }
+    */
+
     RP3D_PROFILE("CollisionDetection::reducePotentialContactManifolds()", mProfiler);
 
     // ----- OLD ----- //
@@ -867,16 +957,16 @@ void CollisionDetection::reducePotentialContactManifolds(const OverlappingPairMa
             // Look for a manifold with the smallest contact penetration depth.
             decimal minDepth = DECIMAL_LARGEST;
             int minDepthManifoldIndex = -1;
-            for (uint i=0; i < contactPair.potentialContactManifoldsIndices.size(); i++) {
+            for (uint j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) {
 
-                ContactManifoldInfo& manifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[i]];
+                ContactManifoldInfo& manifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]];
 
                 // Get the largest contact point penetration depth of the manifold
                 const decimal depth = computePotentialManifoldLargestContactDepth(manifold);
 
                 if (depth < minDepth) {
                     minDepth = depth;
-                    minDepthManifoldIndex = static_cast<int>(i);
+                    minDepthManifoldIndex = static_cast<int>(j);
                 }
             }
 
@@ -892,9 +982,9 @@ void CollisionDetection::reducePotentialContactManifolds(const OverlappingPairMa
         const ContactPair& pairContact = (*mCurrentContactPairs)[i];
 
         // For each potential contact manifold
-        for (uint i=0; i < pairContact.potentialContactManifoldsIndices.size(); i++) {
+        for (uint j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) {
 
-            ContactManifoldInfo& manifold = mPotentialContactManifolds[pairContact.potentialContactManifoldsIndices[i]];
+            ContactManifoldInfo& manifold = mPotentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
 
             // If there are two many contact points in the manifold
             if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) {
@@ -943,6 +1033,9 @@ void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, cons
     // point we want to keep, we will remove it from this list
     List<uint> candidatePointsIndices(manifold.potentialContactPointsIndices);
 
+    // TODO : DELETE THIS
+    uint nbPoints = candidatePointsIndices.size();
+
     int8 nbReducedPoints = 0;
 
     uint pointsToKeepIndices[MAX_CONTACT_POINTS_IN_MANIFOLD];
@@ -1056,6 +1149,8 @@ void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, cons
 
     decimal largestArea = decimal(0.0); // Largest area (positive or negative)
     elementIndexToKeep = 0;
+    nbReducedPoints = 4;
+    decimal area;
 
     // For each remaining candidate points
     for (uint i=0; i < candidatePointsIndices.size(); i++) {
@@ -1067,10 +1162,10 @@ void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, cons
         assert(candidatePointsIndices[i] != pointsToKeepIndices[2]);
 
         // For each edge of the triangle made by the first three points
-        for (uint i=0; i<3; i++) {
+        for (uint j=0; j<3; j++) {
 
-            uint edgeVertex1Index = i;
+            uint edgeVertex1Index = j;
-            uint edgeVertex2Index = i < 2 ? i + 1 : 0;
+            uint edgeVertex2Index = j < 2 ? j + 1 : 0;
 
             const ContactPointInfo& pointToKeepEdgeV1 = mPotentialContactPoints[pointsToKeepIndices[edgeVertex1Index]];
             const ContactPointInfo& pointToKeepEdgeV2 = mPotentialContactPoints[pointsToKeepIndices[edgeVertex2Index]];
@@ -1079,7 +1174,7 @@ void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, cons
             const Vector3 newToSecond = pointToKeepEdgeV2.localPoint1 - element.localPoint1;
 
             // Compute the triangle area
-            const decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
+            area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
 
             // We are looking at the triangle with maximal area (positive or negative).
             // If the previous area is positive, we are looking at negative area now.
@@ -1087,18 +1182,15 @@ void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, cons
             if (isPreviousAreaPositive && area <= largestArea) {
                 largestArea = area;
                 elementIndexToKeep = i;
-                nbReducedPoints = 4;
             }
             else if (!isPreviousAreaPositive && area >= largestArea) {
                 largestArea = area;
                 elementIndexToKeep = i;
-                nbReducedPoints = 4;
             }
         }
     }
     pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
     candidatePointsIndices.removeAt(elementIndexToKeep);
-    assert(nbReducedPoints == 4);
 
     // Only keep the four selected contact points in the manifold
     manifold.potentialContactPointsIndices.clear();

src/collision/narrowphase/NarrowPhaseInfoBatch.cpp

@@ -28,6 +28,7 @@
 #include "collision/ContactPointInfo.h"
 #include "collision/shapes/TriangleShape.h"
 #include "engine/OverlappingPair.h"
+#include <iostream>
 
 using namespace reactphysics3d;
 
@@ -77,6 +78,10 @@ void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNor
     ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
             ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
 
+    // TODO : DELETE THIS
+    //std::cout << "Pair: " << overlappingPairs[index]->getId().first << ", " << overlappingPairs[index]->getId().second << std::endl;
+    //std::cout << ">> Contact Point: " << localPt1.x << ", " << localPt1.y << ", " << localPt1.z << std::endl;
+
     // Add it into the list of contact points
     contactPoints[index].add(contactPointInfo);
 }