diff --git a/include/reactphysics3d/collision/ContactManifold.h b/include/reactphysics3d/collision/ContactManifold.h index 3e4c546a..3dfeb0df 100644 --- a/include/reactphysics3d/collision/ContactManifold.h +++ b/include/reactphysics3d/collision/ContactManifold.h @@ -58,7 +58,7 @@ class ContactManifold { // -------------------- Constants -------------------- // /// Maximum number of contact points in a reduced contact manifold - const int MAX_CONTACT_POINTS_IN_MANIFOLD = 4; + static constexpr int MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // -------------------- Attributes -------------------- // @@ -78,7 +78,7 @@ class ContactManifold { Entity colliderEntity2; /// Number of contacts in the cache - int8 nbContactPoints; + uint8 nbContactPoints; /// First friction vector of the contact manifold Vector3 frictionVector1; @@ -107,16 +107,7 @@ class ContactManifold { /// Constructor ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, - uint contactPointsIndex, int8 nbContactPoints); - - /// Destructor - ~ContactManifold(); - - /// Copy-constructor - ContactManifold(const ContactManifold& contactManifold) = default; - - /// Assignment operator - ContactManifold& operator=(const ContactManifold& contactManifold) = default; + uint contactPointsIndex, uint8 nbContactPoints); // -------------------- Friendship -------------------- // diff --git a/include/reactphysics3d/collision/ContactManifoldInfo.h b/include/reactphysics3d/collision/ContactManifoldInfo.h index 6f6a0e74..b6dd9219 100644 --- a/include/reactphysics3d/collision/ContactManifoldInfo.h +++ b/include/reactphysics3d/collision/ContactManifoldInfo.h @@ -45,8 +45,12 @@ struct ContactManifoldInfo { // -------------------- Attributes -------------------- // + /// Number of potential contact points + uint8 nbPotentialContactPoints; + /// Indices of the contact points in the mPotentialContactPoints array - List potentialContactPointsIndices; + uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; + /// Overlapping pair id uint64 pairId; @@ -54,8 +58,7 @@ struct ContactManifoldInfo { // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator) - : potentialContactPointsIndices(allocator, 4), pairId(pairId) { + ContactManifoldInfo(uint64 pairId) : nbPotentialContactPoints(0), pairId(pairId) { } diff --git a/include/reactphysics3d/collision/ContactPair.h b/include/reactphysics3d/collision/ContactPair.h index f9a49817..abd648fd 100644 --- a/include/reactphysics3d/collision/ContactPair.h +++ b/include/reactphysics3d/collision/ContactPair.h @@ -47,8 +47,11 @@ struct ContactPair { /// Overlapping pair Id uint64 pairId; + /// Number of potential contact manifolds + uint8 nbPotentialContactManifolds; + /// Indices of the potential contact manifolds - List potentialContactManifoldsIndices; + uint potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS]; /// Entity of the first body of the contact Entity body1Entity; @@ -90,13 +93,21 @@ struct ContactPair { /// Constructor ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity, - Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator) - : pairId(pairId), potentialContactManifoldsIndices(allocator, 8), body1Entity(body1Entity), body2Entity(body2Entity), + Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger) + : pairId(pairId), nbPotentialContactManifolds(0), body1Entity(body1Entity), body2Entity(body2Entity), collider1Entity(collider1Entity), collider2Entity(collider2Entity), isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0), contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) { } + + // Remove a potential manifold at a given index in the array + void removePotentialManifoldAtIndex(uint index) { + assert(index < nbPotentialContactManifolds); + + potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1]; + nbPotentialContactManifolds--; + } }; } diff --git a/include/reactphysics3d/collision/ContactPointInfo.h b/include/reactphysics3d/collision/ContactPointInfo.h index cb44673d..387f2363 100644 --- a/include/reactphysics3d/collision/ContactPointInfo.h +++ b/include/reactphysics3d/collision/ContactPointInfo.h @@ -76,9 +76,6 @@ struct ContactPointInfo { assert(contactNormal.lengthSquare() > decimal(0.8)); assert(penDepth > decimal(0.0)); } - - /// Destructor - ~ContactPointInfo() = default; }; } diff --git a/include/reactphysics3d/configuration.h b/include/reactphysics3d/configuration.h index 6283a488..0fbd2b6a 100644 --- a/include/reactphysics3d/configuration.h +++ b/include/reactphysics3d/configuration.h @@ -103,6 +103,15 @@ constexpr decimal PI_TIMES_2 = decimal(6.28318530); /// without triggering a large modification of the tree each frame which can be costly constexpr decimal DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE = decimal(0.08); +/// Maximum number of contact manifolds in an overlapping pair +constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3; + +/// Maximum number of potential contact manifolds in an overlapping pair +constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS; + +/// Maximum number of contact points in potential contact manifold +constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 12; + /// Current version of ReactPhysics3D const std::string RP3D_VERSION = std::string("0.8.0"); diff --git a/include/reactphysics3d/containers/List.h b/include/reactphysics3d/containers/List.h index a85fb65a..140f0b56 100755 --- a/include/reactphysics3d/containers/List.h +++ b/include/reactphysics3d/containers/List.h @@ -294,6 +294,21 @@ class List { mSize++; } + /// Add an element into the list by constructing it directly into the list (in order to avoid a copy) + template + void emplace(Ts&&... args) { + + // If we need to allocate more memory + if (mSize == mCapacity) { + reserve(mCapacity == 0 ? 1 : mCapacity * 2); + } + + // Construct the element directly at its location in the array + new (static_cast(mBuffer) + mSize * sizeof(T)) T(std::forward(args)...); + + mSize++; + } + /// Add a given numbers of elements at the end of the list but do not init them void addWithoutInit(uint nbElements) { diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h index 0d564bd7..c4c841ef 100644 --- a/include/reactphysics3d/engine/PhysicsWorld.h +++ b/include/reactphysics3d/engine/PhysicsWorld.h @@ -116,9 +116,6 @@ class PhysicsWorld { /// might enter sleeping mode decimal defaultSleepAngularVelocity; - /// Maximum number of contact manifolds in an overlapping pair - uint nbMaxContactManifolds; - /// This is used to test if two contact manifold are similar (same contact normal) in order to /// merge them. If the cosine of the angle between the normals of the two manifold are larger /// than the value bellow, the manifold are considered to be similar. @@ -139,7 +136,6 @@ class PhysicsWorld { defaultTimeBeforeSleep = 1.0f; defaultSleepLinearVelocity = decimal(0.02); defaultSleepAngularVelocity = decimal(3.0) * (PI_RP3D / decimal(180.0)); - nbMaxContactManifolds = 3; cosAngleSimilarContactManifold = decimal(0.95); } @@ -163,7 +159,6 @@ class PhysicsWorld { ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl; ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl; ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl; - ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl; ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl; return ss.str(); diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h index 22bda63e..6ae515f6 100644 --- a/include/reactphysics3d/systems/CollisionDetectionSystem.h +++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h @@ -128,19 +128,9 @@ class CollisionDetectionSystem { /// List of lost contact pairs (contact pairs in contact in previous frame but not in the current one) List mLostContactPairs; - /// First map of overlapping pair id to the index of the corresponding pair contact - Map mMapPairIdToContactPairIndex1; - - /// Second map of overlapping pair id to the index of the corresponding pair contact - Map mMapPairIdToContactPairIndex2; - /// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) - Map* mPreviousMapPairIdToContactPairIndex; - - /// Pointer to the map of overlappingPairId to the index of contact pair of the current frame - /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) - Map* mCurrentMapPairIdToContactPairIndex; + Map mPreviousMapPairIdToContactPairIndex; /// First list with the contact manifolds List mContactManifolds1; @@ -223,12 +213,11 @@ class CollisionDetectionSystem { /// Convert the potential contact into actual contacts void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, List* contactPairs); + List& potentialContactManifolds, + Map& mapPairIdToContactPairIndex, List* contactPairs); /// Process the potential contacts after narrow-phase collision detection void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, List* contactPairs); /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts @@ -238,6 +227,9 @@ class CollisionDetectionSystem { /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair void createContacts(); + /// Compute the map from contact pairs ids to contact pair for the next frame + void computeMapPreviousContactPairs(); + /// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one) void computeLostContactPairs(); @@ -277,6 +269,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& convexPairs, List& concavePairs) const; + /// Remove an element in an array (and replace it by the last one in the array) + void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const; + public : // -------------------- Methods -------------------- // diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 10c5c957..46f886de 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -32,14 +32,9 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, - uint contactPointsIndex, int8 nbContactPoints) + uint contactPointsIndex, uint8 nbContactPoints) :contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2), colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0), frictionTwistImpulse(0), isAlreadyInIsland(false) { } - -// Destructor -ContactManifold::~ContactManifold() { - -} diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index a9c73a60..0ef91ca8 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -824,11 +824,11 @@ void PhysicsWorld::createIslands() { if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex) && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) { - assert(pair.potentialContactManifoldsIndices.size() > 0); - nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); + assert(pair.nbPotentialContactManifolds > 0); + nbTotalManifolds += pair.nbPotentialContactManifolds; // Add the contact manifold into the island - mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); + mIslands.nbContactManifolds[islandIndex] += pair.nbPotentialContactManifolds; pair.isAlreadyInIsland = true; // Check if the other body has already been added to the island diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 492b4088..e11f7e86 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -62,13 +62,11 @@ CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, Collider mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()), mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2), - mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), - mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()), mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), - mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2), mContactManifolds1(mMemoryManager.getPoolAllocator()), - mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2), - mContactPoints1(mMemoryManager.getPoolAllocator()), - mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1), - mCurrentContactPoints(&mContactPoints2) { + mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mPreviousMapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator()), + mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()), + mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2), + mContactPoints1(mMemoryManager.getPoolAllocator()), mContactPoints2(mMemoryManager.getPoolAllocator()), + mPreviousContactPoints(&mContactPoints1), mCurrentContactPoints(&mContactPoints2) { #ifdef IS_RP3D_PROFILING_ENABLED @@ -159,7 +157,7 @@ void CollisionDetectionSystem::addLostContactPair(uint64 overlappingPairIndex) { // Create a lost contact pair ContactPair lostContactPair(mOverlappingPairs.mPairIds[overlappingPairIndex], body1Entity, body2Entity, collider1Entity, collider2Entity, mLostContactPairs.size(), - true, isTrigger, mMemoryManager.getHeapAllocator()); + true, isTrigger); mLostContactPairs.add(lostContactPair); } @@ -480,13 +478,14 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow // Process the potential contacts after narrow-phase collision detection void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, List* contactPairs) { assert(contactPairs->size() == 0); assert(mapPairIdToContactPairIndex->size() == 0); + Map mapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator(), mPreviousMapPairIdToContactPairIndex.size()); + // get the narrow-phase batches to test for collision NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch(); @@ -496,18 +495,13 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch(); // Process the potential contacts - processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs); - processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs); - processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs); - processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs); - processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs); - processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs); + processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, + potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); } // Compute the narrow-phase collision detection @@ -527,7 +521,7 @@ void CollisionDetectionSystem::computeNarrowPhase() { testNarrowPhaseCollision(mNarrowPhaseInput, true, allocator); // Process all the potential contacts after narrow-phase collision - processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex, + processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mPotentialContactManifolds, mCurrentContactPairs); // Reduce the number of contact points in the manifolds @@ -539,9 +533,21 @@ void CollisionDetectionSystem::computeNarrowPhase() { // Create the actual narrow-phase contacts createContacts(); + // Compute the map from contact pairs ids to contact pair for the next frame + computeMapPreviousContactPairs(); + mNarrowPhaseInput.clear(); } +/// Compute the map from contact pairs ids to contact pair for the next frame +void CollisionDetectionSystem::computeMapPreviousContactPairs() { + + mPreviousMapPairIdToContactPairIndex.clear(); + for (uint i=0; i < mCurrentContactPairs->size(); i++) { + mPreviousMapPairIdToContactPairIndex.add(Pair((*mCurrentContactPairs)[i].pairId, i)); + } +} + // Compute the narrow-phase collision detection for the testOverlap() methods. /// This method returns true if contacts are found. bool CollisionDetectionSystem::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& narrowPhaseInput, OverlapCallback* callback) { @@ -629,8 +635,7 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; // Create a new contact pair - ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, - contactPairs.size(), isTrigger, false, mMemoryManager.getHeapAllocator()); + ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false); contactPairs.add(contactPair); setOverlapContactPairId.add(narrowPhaseInfoBatch.overlappingPairIds[i]); @@ -657,14 +662,13 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn List potentialContactPoints(allocator); List potentialContactManifolds(allocator); - Map mapPairIdToContactPairIndex(allocator); List contactPairs(allocator); List lostContactPairs(allocator); // Not used during collision snapshots List contactManifolds(allocator); List contactPoints(allocator); // Process all the potential contacts after narrow-phase collision - processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, &contactPairs); + processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, potentialContactManifolds, &contactPairs); // Reduce the number of contact points in the manifolds reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints); @@ -687,24 +691,20 @@ void CollisionDetectionSystem::swapPreviousAndCurrentContacts() { mPreviousContactPairs = &mContactPairs2; mPreviousContactManifolds = &mContactManifolds2; mPreviousContactPoints = &mContactPoints2; - mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2; mCurrentContactPairs = &mContactPairs1; mCurrentContactManifolds = &mContactManifolds1; mCurrentContactPoints = &mContactPoints1; - mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1; } else { mPreviousContactPairs = &mContactPairs1; mPreviousContactManifolds = &mContactManifolds1; mPreviousContactPoints = &mContactPoints1; - mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1; mCurrentContactPairs = &mContactPairs2; mCurrentContactManifolds = &mContactManifolds2; mCurrentContactPoints = &mContactPoints2; - mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2; } } @@ -722,7 +722,7 @@ void CollisionDetectionSystem::createContacts() { ContactPair& contactPair = (*mCurrentContactPairs)[p]; contactPair.contactManifoldsIndex = mCurrentContactManifolds->size(); - contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); + contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; contactPair.contactPointsIndex = mCurrentContactPoints->size(); // Add the associated contact pair to both bodies of the pair (used to create islands later) @@ -734,13 +734,13 @@ void CollisionDetectionSystem::createContacts() { } // For each potential contact manifold of the pair - for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) { + for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) { ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold const uint contactPointsIndex = mCurrentContactPoints->size(); - const int8 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + const int8 nbContactPoints = static_cast(potentialManifold.nbPotentialContactPoints); contactPair.nbToTalContactPoints += nbContactPoints; // We create a new contact manifold @@ -750,10 +750,10 @@ void CollisionDetectionSystem::createContacts() { // Add the contact manifold mCurrentContactManifolds->add(contactManifold); - assert(potentialManifold.potentialContactPointsIndices.size() > 0); + assert(potentialManifold.nbPotentialContactPoints > 0); // For each contact point of the manifold - for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) { + for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) { ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; @@ -775,7 +775,6 @@ void CollisionDetectionSystem::createContacts() { mPreviousContactPoints->clear(); mPreviousContactManifolds->clear(); mPreviousContactPairs->clear(); - mPreviousMapPairIdToContactPairIndex->clear(); // Reset the potential contacts mPotentialContactPoints.clear(true); @@ -817,20 +816,20 @@ void CollisionDetectionSystem::createSnapshotContacts(List& contact for (uint p=0; p < contactPairs.size(); p++) { ContactPair& contactPair = contactPairs[p]; - assert(contactPair.potentialContactManifoldsIndices.size() > 0); + assert(contactPair.nbPotentialContactManifolds > 0); contactPair.contactManifoldsIndex = contactManifolds.size(); - contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); + contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; contactPair.contactPointsIndex = contactPoints.size(); // For each potential contact manifold of the pair - for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) { + for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) { ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold const uint contactPointsIndex = contactPoints.size(); - const int8 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints; contactPair.nbToTalContactPoints += nbContactPoints; // We create a new contact manifold @@ -840,10 +839,10 @@ void CollisionDetectionSystem::createSnapshotContacts(List& contact // Add the contact manifold contactManifolds.add(contactManifold); - assert(potentialManifold.potentialContactPointsIndices.size() > 0); + assert(potentialManifold.nbPotentialContactPoints > 0); // For each contact point of the manifold - for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) { + for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) { ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; @@ -866,10 +865,10 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() { ContactPair& currentContactPair = (*mCurrentContactPairs)[i]; // Find the corresponding contact pair in the previous frame (if any) - auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex->find(currentContactPair.pairId); + auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex.find(currentContactPair.pairId); // If we have found a corresponding contact pair in the previous frame - if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex->end()) { + if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex.end()) { const uint previousContactPairIndex = itPrevContactPair->second; ContactPair& previousContactPair = (*mPreviousContactPairs)[previousContactPairIndex]; @@ -988,8 +987,8 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, // Convert the potential contact into actual contacts void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, + Map& mapPairIdToContactPairIndex, List* contactPairs) { RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler); @@ -1012,100 +1011,151 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na mOverlappingPairs.mCollidingInCurrentFrame[pairIndex] = true; - // If there is not already a contact pair for this overlapping pair - auto it = mapPairIdToContactPairIndex->find(pairId); - ContactPair* pairContact = nullptr; - if (it == mapPairIdToContactPairIndex->end()) { + const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i]; + const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i]; + + const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); + const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); + + const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index]; + const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index]; + + // If we have a convex vs convex collision (if we consider the base collision shapes of the colliders) + if (mCollidersComponents.mCollisionShapes[collider1Index]->isConvex() && + mCollidersComponents.mCollisionShapes[collider2Index]->isConvex()) { // Create a new ContactPair - const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i]; - const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i]; - - const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); - const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); - - const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index]; - const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index]; - const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity)); const uint newContactPairIndex = contactPairs->size(); - ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, - collider1Entity, collider2Entity, - newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger, mMemoryManager.getHeapAllocator()); - contactPairs->add(overlappingPairContact); - pairContact = &((*contactPairs)[newContactPairIndex]); - mapPairIdToContactPairIndex->add(Pair(pairId, newContactPairIndex)); - } - else { // If a ContactPair already exists for this overlapping pair, we use this one - assert(it->first == pairId); + contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, + newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger); - const uint pairContactIndex = it->second; - pairContact = &((*contactPairs)[pairContactIndex]); - } + ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]); - assert(pairContact != nullptr); + // Create a new potential contact manifold for the overlapping pair + uint contactManifoldIndex = static_cast(potentialContactManifolds.size()); + potentialContactManifolds.emplace(pairId); + ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; - // Add the potential contacts - for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) { + const uint contactPointIndexStart = static_cast(potentialContactPoints.size()); - const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]); + // Add the potential contacts + for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) { - // Add the contact point to the list of potential contact points - const uint contactPointIndex = static_cast(potentialContactPoints.size()); - - potentialContactPoints.add(contactPoint); - - bool similarManifoldFound = false; - - // 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 - assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0); - const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0]; - const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex]; - - // If we have found a corresponding manifold for the new contact point - // (a manifold with a similar contact normal direction) - if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) { + if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { // Add the contact point to the manifold - potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex); + contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j; + contactManifoldInfo.nbPotentialContactPoints++; - similarManifoldFound = true; + // Add the contact point to the list of potential contact points + const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]); - break; + potentialContactPoints.add(contactPoint); } } - // If we have not found a manifold with a similar contact normal for the contact point - if (!similarManifoldFound) { + // Add the contact manifold to the overlapping pair contact + assert(overlappingPairContact.pairId == contactManifoldInfo.pairId); + pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex; + pairContact->nbPotentialContactManifolds = 1; + } + else { - // Create a new contact manifold for the overlapping pair - ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator()); + // If there is not already a contact pair for this overlapping pair + auto it = mapPairIdToContactPairIndex.find(pairId); + ContactPair* pairContact = nullptr; + if (it == mapPairIdToContactPairIndex.end()) { - // Add the contact point to the manifold - contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex); + // Create a new ContactPair - assert(pairContact != nullptr); + const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; - // Add the potential contact manifold - uint contactManifoldIndex = static_cast(potentialContactManifolds.size()); - potentialContactManifolds.add(contactManifoldInfo); + assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity)); - // Add the contact manifold to the overlapping pair contact - assert(pairContact->pairId == contactManifoldInfo.pairId); - pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex); + const uint newContactPairIndex = contactPairs->size(); + contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, + newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger); + pairContact = &((*contactPairs)[newContactPairIndex]); + mapPairIdToContactPairIndex.add(Pair(pairId, newContactPairIndex)); + + } + else { // If a ContactPair already exists for this overlapping pair, we use this one + + assert(it->first == pairId); + + const uint pairContactIndex = it->second; + pairContact = &((*contactPairs)[pairContactIndex]); } - assert(pairContact->potentialContactManifoldsIndices.size() > 0); + assert(pairContact != nullptr); + + // Add the potential contacts + for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) { + + const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]); + + // Add the contact point to the list of potential contact points + const uint contactPointIndex = static_cast(potentialContactPoints.size()); + + potentialContactPoints.add(contactPoint); + + bool similarManifoldFound = false; + + // For each contact manifold of the overlapping pair + for (uint m=0; m < pairContact->nbPotentialContactManifolds; m++) { + + uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m]; + + if (potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { + + // Get the first contact point of the current manifold + assert(potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints > 0); + const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0]; + const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex]; + + // If we have found a corresponding manifold for the new contact point + // (a manifold with a similar contact normal direction) + if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) { + + // Add the contact point to the manifold + potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex; + potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++; + + similarManifoldFound = true; + + break; + } + } + } + + // If we have not found a manifold with a similar contact normal for the contact point + if (!similarManifoldFound && pairContact->nbPotentialContactManifolds < NB_MAX_POTENTIAL_CONTACT_MANIFOLDS) { + + // Create a new potential contact manifold for the overlapping pair + uint contactManifoldIndex = static_cast(potentialContactManifolds.size()); + potentialContactManifolds.emplace(pairId); + ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; + + // Add the contact point to the manifold + contactManifoldInfo.potentialContactPointsIndices[0] = contactPointIndex; + contactManifoldInfo.nbPotentialContactPoints = 1; + + assert(pairContact != nullptr); + + // Add the contact manifold to the overlapping pair contact + assert(pairContact->pairId == contactManifoldInfo.pairId); + pairContact->potentialContactManifoldsIndices[pairContact->nbPotentialContactManifolds] = contactManifoldIndex; + pairContact->nbPotentialContactManifolds++; + } + + assert(pairContact->nbPotentialContactManifolds > 0); + } } narrowPhaseInfoBatch.resetContactPoints(i); @@ -1126,12 +1176,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List ContactPair& contactPair = (*contactPairs)[i]; // While there are too many manifolds in the contact pair - while(contactPair.potentialContactManifoldsIndices.size() > mWorld->mConfig.nbMaxContactManifolds) { + while(contactPair.nbPotentialContactManifolds > NB_MAX_CONTACT_MANIFOLDS) { // Look for a manifold with the smallest contact penetration depth. decimal minDepth = DECIMAL_LARGEST; int minDepthManifoldIndex = -1; - for (uint j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) { + for (uint j=0; j < contactPair.nbPotentialContactManifolds; j++) { ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]]; @@ -1146,7 +1196,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List // Remove the non optimal manifold assert(minDepthManifoldIndex >= 0); - contactPair.potentialContactManifoldsIndices.removeAt(minDepthManifoldIndex); + contactPair.removePotentialManifoldAtIndex(minDepthManifoldIndex); } } @@ -1156,12 +1206,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List const ContactPair& pairContact = (*contactPairs)[i]; // For each potential contact manifold - for (uint j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) { + for (uint j=0; j < pairContact.nbPotentialContactManifolds; j++) { ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]]; // If there are two many contact points in the manifold - if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) { + if (manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { Entity collider1 = mOverlappingPairs.mColliders1[mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]]; @@ -1171,7 +1221,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints); } - assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); } } } @@ -1182,9 +1232,9 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co decimal largestDepth = 0.0f; - assert(manifold.potentialContactPointsIndices.size() > 0); + assert(manifold.nbPotentialContactPoints > 0); - for (uint i=0; i < manifold.potentialContactPointsIndices.size(); i++) { + for (uint i=0; i < manifold.nbPotentialContactPoints; i++) { decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth; if (depth > largestDepth) { @@ -1202,14 +1252,18 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, const List& potentialContactPoints) const { - assert(manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD); // The following algorithm only works to reduce to a maximum of 4 contact points assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); // List of the candidate contact points indices in the manifold. Every time that we have found a // point we want to keep, we will remove it from this list - List candidatePointsIndices(manifold.potentialContactPointsIndices); + uint candidatePointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; + uint8 nbCandidatePoints = manifold.nbPotentialContactPoints; + for (uint8 i=0 ; i < manifold.nbPotentialContactPoints; i++) { + candidatePointsIndices[i] = manifold.potentialContactPointsIndices[i]; + } int8 nbReducedPoints = 0; @@ -1233,7 +1287,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold const Vector3 searchDirection(1, 1, 1); decimal maxDotProduct = DECIMAL_SMALLEST; uint elementIndexToKeep = 0; - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; decimal dotProduct = searchDirection.dot(element.localPoint1); @@ -1244,7 +1298,8 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep]; - candidatePointsIndices.removeAt(elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); + //candidatePointsIndices.removeAt(elementIndexToKeep); assert(nbReducedPoints == 1); // Compute the second contact point we need to keep. @@ -1252,7 +1307,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal maxDistance = decimal(0.0); elementIndexToKeep = 0; - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1268,7 +1323,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep]; - candidatePointsIndices.removeAt(elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); assert(nbReducedPoints == 2); // Compute the third contact point we need to keep. @@ -1281,7 +1336,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal minArea = decimal(0.0); decimal maxArea = decimal(0.0); bool isPreviousAreaPositive = true; - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1310,12 +1365,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold if (maxArea > (-minArea)) { isPreviousAreaPositive = true; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex]; - candidatePointsIndices.removeAt(thirdPointMaxAreaIndex); + removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints); } else { isPreviousAreaPositive = false; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex]; - candidatePointsIndices.removeAt(thirdPointMinAreaIndex); + removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints); } nbReducedPoints = 3; @@ -1328,7 +1383,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal area; // For each remaining candidate points - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; @@ -1365,14 +1420,22 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep]; - candidatePointsIndices.removeAt(elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); // Only keep the four selected contact points in the manifold - manifold.potentialContactPointsIndices.clear(); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]); + manifold.potentialContactPointsIndices[0] = pointsToKeepIndices[0]; + manifold.potentialContactPointsIndices[1] = pointsToKeepIndices[1]; + manifold.potentialContactPointsIndices[2] = pointsToKeepIndices[2]; + manifold.potentialContactPointsIndices[3] = pointsToKeepIndices[3]; + manifold.nbPotentialContactPoints = 4; +} + +// Remove an element in an array (and replace it by the last one in the array) +void CollisionDetectionSystem::removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const { + assert(index < arraySize); + assert(arraySize > 0); + array[index] = array[arraySize - 1]; + arraySize--; } // Report contacts and triggers