Fix issue with contact manifolds order in islands creation process

include/reactphysics3d/engine/PhysicsWorld.h

@@ -241,6 +241,11 @@ class PhysicsWorld {
         /// All the islands of bodies of the current frame
         Islands mIslands;
 
+        /// Order in which to process the ContactPairs for contact creation such that
+        /// all the contact manifolds and contact points of a given island are packed together
+        /// This array contains the indices of the ContactPairs.
+        List<uint32> mProcessContactPairsOrderIslands;
+
         /// Contact solver system
         ContactSolverSystem mContactSolverSystem;
 

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -237,9 +237,6 @@ class CollisionDetectionSystem {
         void reducePotentialContactManifolds(List<ContactPair>* contactPairs, List<ContactManifoldInfo>& potentialContactManifolds,
                                              const List<ContactPointInfo>& potentialContactPoints) const;
 
-        /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
-        void createContacts();
-
         /// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)
         void computeLostContactPairs();
 
@@ -279,6 +276,9 @@ class CollisionDetectionSystem {
         /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
         void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List<uint64>& convexPairs, List<uint64>& concavePairs) const;
 
+        /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
+        void createContacts();
+
     public :
 
         // -------------------- Methods -------------------- //

src/engine/PhysicsWorld.cpp

@@ -59,7 +59,7 @@ PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, const WorldSettings& wo
                 mSliderJointsComponents(mMemoryManager.getHeapAllocator()), mCollisionDetection(this, mCollidersComponents, mTransformComponents, mCollisionBodyComponents, mRigidBodyComponents,
                                         mMemoryManager),
                 mCollisionBodies(mMemoryManager.getHeapAllocator()), mEventListener(nullptr),
-                mName(worldSettings.worldName),  mIslands(mMemoryManager.getSingleFrameAllocator()),
+                mName(worldSettings.worldName),  mIslands(mMemoryManager.getSingleFrameAllocator()), mProcessContactPairsOrderIslands(mMemoryManager.getSingleFrameAllocator()),
                 mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
                                mCollidersComponents, mConfig.restitutionVelocityThreshold),
                 mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
@@ -342,6 +342,9 @@ void PhysicsWorld::update(decimal timeStep) {
     // Create the islands
     createIslands();
 
+    // Create the actual narrow-phase contacts
+    mCollisionDetection.createContacts();
+
     // Report the contacts to the user
     mCollisionDetection.reportContactsAndTriggers();
 
@@ -374,6 +377,8 @@ void PhysicsWorld::update(decimal timeStep) {
     // Reset the islands
     mIslands.clear();
 
+    mProcessContactPairsOrderIslands.clear(true);
+
     // Generate debug rendering primitives (if enabled)
     if (mIsDebugRenderingEnabled) {
         mDebugRenderer.computeDebugRenderingPrimitives(*this);
@@ -747,6 +752,8 @@ void PhysicsWorld::createIslands() {
 
     RP3D_PROFILE("PhysicsWorld::createIslands()", mProfiler);
 
+    assert(mProcessContactPairsOrderIslands.size() == 0);
+
     // Reset all the isAlreadyInIsland variables of bodies and joints
     for (uint b=0; b < mRigidBodyComponents.getNbComponents(); b++) {
 
@@ -814,6 +821,8 @@ void PhysicsWorld::createIslands() {
                     if (mRigidBodyComponents.hasComponent(pair.body1Entity) && mRigidBodyComponents.hasComponent(pair.body2Entity)
                         && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) {
 
+                        mProcessContactPairsOrderIslands.add(contactPairs[p]);
+
                         assert(pair.potentialContactManifoldsIndices.size() > 0);
                         nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
 
@@ -871,6 +880,15 @@ void PhysicsWorld::createIslands() {
         }
     }
 
+    for (uint32 i=0; i < (*mCollisionDetection.mCurrentContactPairs).size(); i++) {
+
+        ContactPair& contactPair = (*mCollisionDetection.mCurrentContactPairs)[i];
+
+        if (mRigidBodyComponents.hasComponent(contactPair.body1Entity) && mRigidBodyComponents.hasComponent(contactPair.body2Entity)) {
+
+        }
+    }
+
     mCollisionDetection.mMapBodyToContactPairs.clear(true);
 }
 

src/systems/CollisionDetectionSystem.cpp

@@ -532,11 +532,6 @@ void CollisionDetectionSystem::computeNarrowPhase() {
 
     assert(mCurrentContactManifolds->size() == 0);
     assert(mCurrentContactPoints->size() == 0);
-
-    // Create the actual narrow-phase contacts
-    createContacts();
-
-    mNarrowPhaseInput.clear();
 }
 
 // Compute the narrow-phase collision detection for the testOverlap() methods.
@@ -716,10 +711,30 @@ void CollisionDetectionSystem::createContacts() {
     mCurrentContactManifolds->reserve(mCurrentContactPairs->size());
     mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
 
-    // For each contact pair
-    for (uint p=0; p < (*mCurrentContactPairs).size(); p++) {
+    // We go through all the contact pairs and add the pairs with a least a CollisionBody at the end of the
+    // mProcessContactPairsOrderIslands array because those pairs have not been added during the islands
+    // creation (only the pairs with two RigidBodies are added during island creation)
+    Set<uint32> processedContactPairsIndices(mMemoryManager.getSingleFrameAllocator(), mCurrentContactPairs->size());
+    for (uint32 i=0; i < mWorld->mProcessContactPairsOrderIslands.size(); i++) {
+        processedContactPairsIndices.add(mWorld->mProcessContactPairsOrderIslands[i]);
+    }
+    for (uint32 i=0; i < mCurrentContactPairs->size(); i++) {
+        if (!processedContactPairsIndices.contains(i)) {
+            mWorld->mProcessContactPairsOrderIslands.add(i);
+        }
+    }
 
-        ContactPair& contactPair = (*mCurrentContactPairs)[p];
+    assert(mWorld->mProcessContactPairsOrderIslands.size() == (*mCurrentContactPairs).size());
+
+    // Process the contact pairs in the order defined by the islands such that the contact manifolds and
+    // contact points of a given island are packed together in the array of manifolds and contact points
+    for (uint p=0; p < mWorld->mProcessContactPairsOrderIslands.size(); p++) {
+
+        uint32 contactPairIndex = mWorld->mProcessContactPairsOrderIslands[p];
+
+        processedContactPairsIndices.add(contactPairIndex);
+
+        ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
 
         contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
         contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
@@ -772,6 +787,8 @@ void CollisionDetectionSystem::createContacts() {
     // Reset the potential contacts
     mPotentialContactPoints.clear(true);
     mPotentialContactManifolds.clear(true);
+
+    mNarrowPhaseInput.clear();
 }
 
 // Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)