diff --git a/include/reactphysics3d/collision/ContactManifoldInfo.h b/include/reactphysics3d/collision/ContactManifoldInfo.h index b6dd9219..a843f5bd 100644 --- a/include/reactphysics3d/collision/ContactManifoldInfo.h +++ b/include/reactphysics3d/collision/ContactManifoldInfo.h @@ -45,12 +45,11 @@ struct ContactManifoldInfo { // -------------------- Attributes -------------------- // - /// Number of potential contact points - uint8 nbPotentialContactPoints; + /// Memory allocator + MemoryAllocator& allocator; /// Indices of the contact points in the mPotentialContactPoints array - uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; - + Array potentialContactPointsIndices; /// Overlapping pair id uint64 pairId; @@ -58,7 +57,8 @@ struct ContactManifoldInfo { // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(uint64 pairId) : nbPotentialContactPoints(0), pairId(pairId) { + ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator) + : allocator(allocator), potentialContactPointsIndices(allocator, 4), pairId(pairId) { } diff --git a/include/reactphysics3d/collision/ContactPair.h b/include/reactphysics3d/collision/ContactPair.h index 53e4cbfb..208efdb7 100644 --- a/include/reactphysics3d/collision/ContactPair.h +++ b/include/reactphysics3d/collision/ContactPair.h @@ -44,14 +44,14 @@ struct ContactPair { // -------------------- Attributes -------------------- // + /// Reference to the memory allocator + MemoryAllocator& allocator; + /// Overlapping pair Id uint64 pairId; - /// Number of potential contact manifolds - uint8 nbPotentialContactManifolds; - /// Indices of the potential contact manifolds - uint potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS]; + Array potentialContactManifoldsIndices; /// Entity of the first body of the contact Entity body1Entity; @@ -93,8 +93,9 @@ struct ContactPair { /// Constructor ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity, - Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger) - : pairId(pairId), nbPotentialContactManifolds(0), body1Entity(body1Entity), body2Entity(body2Entity), + Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator) + : allocator(allocator), pairId(pairId), potentialContactManifoldsIndices(allocator, 1), + body1Entity(body1Entity), body2Entity(body2Entity), collider1Entity(collider1Entity), collider2Entity(collider2Entity), isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0), contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) { @@ -103,10 +104,9 @@ struct ContactPair { // Remove a potential manifold at a given index in the array void removePotentialManifoldAtIndex(uint index) { - assert(index < nbPotentialContactManifolds); + assert(index < potentialContactManifoldsIndices.size()); - potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1]; - nbPotentialContactManifolds--; + potentialContactManifoldsIndices.removeAtAndReplaceByLast(index); } }; diff --git a/include/reactphysics3d/configuration.h b/include/reactphysics3d/configuration.h index be47bc7b..03116761 100644 --- a/include/reactphysics3d/configuration.h +++ b/include/reactphysics3d/configuration.h @@ -120,11 +120,8 @@ constexpr uint8 NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO = 16; /// 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 = 16; +/// Distance threshold to consider that two contact points in a manifold are the same +constexpr decimal SAME_CONTACT_POINT_DISTANCE_THRESHOLD = decimal(0.01); /// Current version of ReactPhysics3D const std::string RP3D_VERSION = std::string("0.8.0"); diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h index a978fd3a..39736c5d 100644 --- a/include/reactphysics3d/systems/CollisionDetectionSystem.h +++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h @@ -265,6 +265,10 @@ class CollisionDetectionSystem { void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, const Array& potentialContactPoints) const; + /// Remove the duplicated contact points in a given contact manifold + void removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold, + const Array& potentialContactPoints) const; + /// Report contacts void reportContacts(CollisionCallback& callback, Array* contactPairs, Array* manifolds, Array* contactPoints, Array& lostContactPairs); @@ -289,7 +293,7 @@ class CollisionDetectionSystem { void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, Array& convexPairs, Array& 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; + void removeItemAtInArray(Array& array, uint32 index) const; public : diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 6a2689f6..721ce13a 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -845,11 +845,11 @@ void PhysicsWorld::createIslands() { mProcessContactPairsOrderIslands.add(contactPairIndex); - assert(pair.nbPotentialContactManifolds > 0); - nbTotalManifolds += pair.nbPotentialContactManifolds; + assert(pair.potentialContactManifoldsIndices.size() > 0); + nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); // Add the contact manifold into the island - mIslands.nbContactManifolds[islandIndex] += pair.nbPotentialContactManifolds; + mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); 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 2124f812..c24f0e87 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -192,7 +192,7 @@ void CollisionDetectionSystem::addLostContactPair(OverlappingPairs::OverlappingP // Create a lost contact pair ContactPair lostContactPair(overlappingPair.pairID, body1Entity, body2Entity, overlappingPair.collider1, overlappingPair.collider2, mLostContactPairs.size(), - true, isTrigger); + true, isTrigger, mMemoryManager.getHeapAllocator()); mLostContactPairs.add(lostContactPair); } @@ -742,7 +742,7 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; // Create a new contact pair - ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false); + ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false, mMemoryManager.getHeapAllocator()); contactPairs.add(contactPair); setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId); @@ -840,27 +840,27 @@ void CollisionDetectionSystem::createContacts() { ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex]; contactPair.contactManifoldsIndex = mCurrentContactManifolds->size(); - contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; + contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); contactPair.contactPointsIndex = mCurrentContactPoints->size(); // For each potential contact manifold of the pair - for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) { + for (uint m=0; m < contactPair.nbContactManifolds; m++) { ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold const uint32 contactPointsIndex = mCurrentContactPoints->size(); - const int8 nbContactPoints = static_cast(potentialManifold.nbPotentialContactPoints); + const uint32 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); contactPair.nbToTalContactPoints += nbContactPoints; // Create and add the contact manifold mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - assert(potentialManifold.nbPotentialContactPoints > 0); + assert(potentialManifold.potentialContactPointsIndices.size() > 0); // For each contact point of the manifold - for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) { + for (uint c=0; c < nbContactPoints; c++) { ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; @@ -948,30 +948,30 @@ void CollisionDetectionSystem::createSnapshotContacts(Array& contac for (uint32 p=0; p < nbContactPairs; p++) { ContactPair& contactPair = contactPairs[p]; - assert(contactPair.nbPotentialContactManifolds > 0); + assert(contactPair.potentialContactManifoldsIndices.size() > 0); contactPair.contactManifoldsIndex = contactManifolds.size(); - contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; + contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); contactPair.contactPointsIndex = contactPoints.size(); // For each potential contact manifold of the pair - for (uint32 m=0; m < contactPair.nbPotentialContactManifolds; m++) { + for (uint32 m=0; m < contactPair.nbContactManifolds; m++) { ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold const uint32 contactPointsIndex = contactPoints.size(); - const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints; + const uint32 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); contactPair.nbToTalContactPoints += nbContactPoints; // Create and add the contact manifold contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - assert(potentialManifold.nbPotentialContactPoints > 0); + assert(nbContactPoints > 0); // For each contact point of the manifold - for (uint32 c=0; c < potentialManifold.nbPotentialContactPoints; c++) { + for (uint32 c=0; c < nbContactPoints; c++) { ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; @@ -1171,13 +1171,13 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na const uint32 newContactPairIndex = contactPairs->size(); contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, - newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger); + newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator()); ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]); // Create a new potential contact manifold for the overlapping pair uint32 contactManifoldIndex = static_cast(potentialContactManifolds.size()); - potentialContactManifolds.emplace(pairId); + potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator()); ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; const uint32 contactPointIndexStart = static_cast(potentialContactPoints.size()); @@ -1185,23 +1185,18 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na // Add the potential contacts for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { - if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { + // Add the contact point to the manifold + contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndexStart + j); - // Add the contact point to the manifold - contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j; - contactManifoldInfo.nbPotentialContactPoints++; + // Add the contact point to the array of potential contact points + const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; - // Add the contact point to the array of potential contact points - const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; - - potentialContactPoints.add(contactPoint); - } + potentialContactPoints.add(contactPoint); } // Add the contact manifold to the overlapping pair contact assert(pairId == contactManifoldInfo.pairId); - pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex; - pairContact->nbPotentialContactManifolds = 1; + pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex); } else { @@ -1218,7 +1213,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na const uint32 newContactPairIndex = contactPairs->size(); contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, - newContactPairIndex, overlappingPair->collidingInPreviousFrame , isTrigger); + newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator()); pairContact = &((*contactPairs)[newContactPairIndex]); mapPairIdToContactPairIndex.add(Pair(pairId, newContactPairIndex)); @@ -1246,53 +1241,48 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na bool similarManifoldFound = false; // For each contact manifold of the overlapping pair - for (uint m=0; m < pairContact->nbPotentialContactManifolds; m++) { + const uint32 nbPotentialManifolds = pairContact->potentialContactManifoldsIndices.size(); + for (uint m=0; m < nbPotentialManifolds; 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].potentialContactPointsIndices.size() > 0); + const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0]; + const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex]; - // 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) { - // 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.add(contactPointIndex); - // Add the contact point to the manifold - potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex; - potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++; + similarManifoldFound = true; - similarManifoldFound = true; - - break; - } - } + 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) { + if (!similarManifoldFound) { // Create a new potential contact manifold for the overlapping pair uint32 contactManifoldIndex = potentialContactManifolds.size(); - potentialContactManifolds.emplace(pairId); + potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator()); ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; // Add the contact point to the manifold - contactManifoldInfo.potentialContactPointsIndices[0] = contactPointIndex; - contactManifoldInfo.nbPotentialContactPoints = 1; + contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex); assert(pairContact != nullptr); // Add the contact manifold to the overlapping pair contact assert(pairContact->pairId == contactManifoldInfo.pairId); - pairContact->potentialContactManifoldsIndices[pairContact->nbPotentialContactManifolds] = contactManifoldIndex; - pairContact->nbPotentialContactManifolds++; + pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex); } - assert(pairContact->nbPotentialContactManifolds > 0); + assert(pairContact->potentialContactManifoldsIndices.size() > 0); } } @@ -1315,12 +1305,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array NB_MAX_CONTACT_MANIFOLDS) { + while(contactPair.potentialContactManifoldsIndices.size() > NB_MAX_CONTACT_MANIFOLDS) { // Look for a manifold with the smallest contact penetration depth. decimal minDepth = DECIMAL_LARGEST; int minDepthManifoldIndex = -1; - for (uint32 j=0; j < contactPair.nbPotentialContactManifolds; j++) { + for (uint32 j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) { ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]]; @@ -1345,12 +1335,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array MAX_CONTACT_POINTS_IN_MANIFOLD) { + if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) { Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(pairContact.collider1Entity); @@ -1358,20 +1348,25 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array& potentialContactPoints) const { + const Array& potentialContactPoints) const { decimal largestDepth = 0.0f; - assert(manifold.nbPotentialContactPoints > 0); + const uint32 nbContactPoints = static_cast(manifold.potentialContactPointsIndices.size()); - for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) { + assert(nbContactPoints > 0); + + for (uint32 i=0; i < nbContactPoints; i++) { decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth; if (depth > largestDepth) { @@ -1389,18 +1384,15 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, const Array& potentialContactPoints) const { - assert(manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(manifold.potentialContactPointsIndices.size() > 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); // Array 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 array - 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]; - } + Array candidatePointsIndices(manifold.potentialContactPointsIndices); + assert(manifold.potentialContactPointsIndices.size() == candidatePointsIndices.size()); int8 nbReducedPoints = 0; @@ -1424,7 +1416,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold const Vector3 searchDirection(1, 1, 1); decimal maxDotProduct = DECIMAL_SMALLEST; uint elementIndexToKeep = 0; - for (uint i=0; i < nbCandidatePoints; i++) { + for (uint i=0; i < candidatePointsIndices.size(); i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; decimal dotProduct = searchDirection.dot(element.localPoint1); @@ -1435,7 +1427,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep]; - removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); //candidatePointsIndices.removeAt(elementIndexToKeep); assert(nbReducedPoints == 1); @@ -1444,7 +1436,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal maxDistance = decimal(0.0); elementIndexToKeep = 0; - for (uint i=0; i < nbCandidatePoints; i++) { + for (uint i=0; i < candidatePointsIndices.size(); i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1460,7 +1452,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep]; - removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); assert(nbReducedPoints == 2); // Compute the third contact point we need to keep. @@ -1473,7 +1465,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal minArea = decimal(0.0); decimal maxArea = decimal(0.0); bool isPreviousAreaPositive = true; - for (uint i=0; i < nbCandidatePoints; i++) { + for (uint i=0; i < candidatePointsIndices.size(); i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1502,12 +1494,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold if (maxArea > (-minArea)) { isPreviousAreaPositive = true; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex]; - removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints); + removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex); } else { isPreviousAreaPositive = false; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex]; - removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints); + removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex); } nbReducedPoints = 3; @@ -1520,7 +1512,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal area; // For each remaining candidate points - for (uint i=0; i < nbCandidatePoints; i++) { + for (uint i=0; i < candidatePointsIndices.size(); i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; @@ -1557,22 +1549,51 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep]; - removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); // Only keep the four selected contact points in the manifold - manifold.potentialContactPointsIndices[0] = pointsToKeepIndices[0]; - manifold.potentialContactPointsIndices[1] = pointsToKeepIndices[1]; - manifold.potentialContactPointsIndices[2] = pointsToKeepIndices[2]; - manifold.potentialContactPointsIndices[3] = pointsToKeepIndices[3]; - manifold.nbPotentialContactPoints = 4; + manifold.potentialContactPointsIndices.clear(); + manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]); + manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]); + manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]); + manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]); +} + +// Remove the duplicated contact points in a given contact manifold +void CollisionDetectionSystem::removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold, + const Array& potentialContactPoints) const { + + RP3D_PROFILE("CollisionDetectionSystem::removeDuplicatedContactPointsInManifold()", mProfiler); + + const decimal distThresholdSqr = SAME_CONTACT_POINT_DISTANCE_THRESHOLD * SAME_CONTACT_POINT_DISTANCE_THRESHOLD; + + // For each contact point of the manifold + for (uint32 i=0; i < manifold.potentialContactPointsIndices.size(); i++) { + for (uint32 j=i+1; j < manifold.potentialContactPointsIndices.size(); j++) { + + const ContactPointInfo& point1 = potentialContactPoints[manifold.potentialContactPointsIndices[i]]; + const ContactPointInfo& point2 = potentialContactPoints[manifold.potentialContactPointsIndices[j]]; + + // Compute the distance between the two contact points + const decimal distSqr = (point2.localPoint1 - point1.localPoint1).lengthSquare(); + + // We have a found a duplicated contact point + if (distSqr < distThresholdSqr) { + + // Remove the duplicated contact point + manifold.potentialContactPointsIndices.removeAtAndReplaceByLast(j); + + j--; + } + } + } } // 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--; +void CollisionDetectionSystem::removeItemAtInArray(Array& array, uint32 index) const { + assert(index < array.size()); + assert(array.size() > 0); + array.removeAtAndReplaceByLast(index); } // Report contacts and triggers