From a0c2c9fe23d974e1c803b747fdf8a5b8cf0b50cf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 28 Sep 2021 21:51:43 +0200 Subject: [PATCH] Merge conflicts when reverting commit a8f939c7 --- .../collision/ContactManifoldInfo.h | 10 +- .../reactphysics3d/collision/ContactPair.h | 18 +- include/reactphysics3d/configuration.h | 7 +- .../systems/CollisionDetectionSystem.h | 6 +- src/engine/PhysicsWorld.cpp | 6 +- src/systems/CollisionDetectionSystem.cpp | 189 ++++++++---------- 6 files changed, 107 insertions(+), 129 deletions(-) diff --git a/include/reactphysics3d/collision/ContactManifoldInfo.h b/include/reactphysics3d/collision/ContactManifoldInfo.h index a843f5bd..b6dd9219 100644 --- a/include/reactphysics3d/collision/ContactManifoldInfo.h +++ b/include/reactphysics3d/collision/ContactManifoldInfo.h @@ -45,11 +45,12 @@ struct ContactManifoldInfo { // -------------------- Attributes -------------------- // - /// Memory allocator - MemoryAllocator& allocator; + /// Number of potential contact points + uint8 nbPotentialContactPoints; /// Indices of the contact points in the mPotentialContactPoints array - Array potentialContactPointsIndices; + uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; + /// Overlapping pair id uint64 pairId; @@ -57,8 +58,7 @@ struct ContactManifoldInfo { // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator) - : allocator(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 8ea7d4bc..5c3688b4 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 - Array potentialContactManifoldsIndices; + uint32 potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS]; /// Entity of the first body of the contact Entity body1Entity; @@ -93,9 +93,8 @@ struct ContactPair { /// Constructor ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity, - Entity collider2Entity, uint32 contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator) - : allocator(allocator), pairId(pairId), potentialContactManifoldsIndices(allocator, 1), - body1Entity(body1Entity), body2Entity(body2Entity), + Entity collider2Entity, uint32 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) { @@ -104,9 +103,10 @@ struct ContactPair { // Remove a potential manifold at a given index in the array void removePotentialManifoldAtIndex(uint32 index) { - assert(index < potentialContactManifoldsIndices.size()); + assert(index < nbPotentialContactManifolds); - potentialContactManifoldsIndices.removeAtAndReplaceByLast(index); + potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1]; + nbPotentialContactManifolds--; } }; diff --git a/include/reactphysics3d/configuration.h b/include/reactphysics3d/configuration.h index 03116761..be47bc7b 100644 --- a/include/reactphysics3d/configuration.h +++ b/include/reactphysics3d/configuration.h @@ -120,8 +120,11 @@ 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; -/// Distance threshold to consider that two contact points in a manifold are the same -constexpr decimal SAME_CONTACT_POINT_DISTANCE_THRESHOLD = decimal(0.01); +/// 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; /// 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 4abd107c..a318d7dd 100644 --- a/include/reactphysics3d/systems/CollisionDetectionSystem.h +++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h @@ -265,10 +265,6 @@ 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); @@ -293,7 +289,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(Array& array, uint32 index) const; + void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const; public : diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 913a9bcb..540fb2a5 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -850,11 +850,11 @@ void PhysicsWorld::createIslands() { mProcessContactPairsOrderIslands.add(contactPairIndex); - 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 de324f55..cdbe20b4 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, mMemoryManager.getHeapAllocator()); + true, isTrigger); 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, mMemoryManager.getHeapAllocator()); + ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false); 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.potentialContactManifoldsIndices.size(); + contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; contactPair.contactPointsIndex = mCurrentContactPoints->size(); // For each potential contact manifold of the pair - for (uint32 m=0; m < contactPair.nbContactManifolds; m++) { + for (uint32 m=0; m < contactPair.nbPotentialContactManifolds; m++) { ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold const uint32 contactPointsIndex = mCurrentContactPoints->size(); - const uint32 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + const int8 nbContactPoints = static_cast(potentialManifold.nbPotentialContactPoints); contactPair.nbToTalContactPoints += nbContactPoints; // Create and add the contact manifold mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - assert(potentialManifold.potentialContactPointsIndices.size() > 0); + assert(potentialManifold.nbPotentialContactPoints > 0); // For each contact point of the manifold - for (uint32 c=0; c < nbContactPoints; c++) { + for (uint32 c=0; c < potentialManifold.nbPotentialContactPoints; 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.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 (uint32 m=0; m < contactPair.nbContactManifolds; m++) { + for (uint32 m=0; m < contactPair.nbPotentialContactManifolds; m++) { ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold const uint32 contactPointsIndex = contactPoints.size(); - const uint32 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints; contactPair.nbToTalContactPoints += nbContactPoints; // Create and add the contact manifold contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - assert(nbContactPoints > 0); + assert(potentialManifold.nbPotentialContactPoints > 0); // For each contact point of the manifold - for (uint32 c=0; c < nbContactPoints; c++) { + for (uint32 c=0; c < potentialManifold.nbPotentialContactPoints; 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, mMemoryManager.getHeapAllocator()); + newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger); ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]); // Create a new potential contact manifold for the overlapping pair uint32 contactManifoldIndex = static_cast(potentialContactManifolds.size()); - potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator()); + potentialContactManifolds.emplace(pairId); ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; const uint32 contactPointIndexStart = static_cast(potentialContactPoints.size()); @@ -1185,18 +1185,23 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na // Add the potential contacts for (uint32 j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { - // Add the contact point to the manifold - contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndexStart + j); + if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { - // 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 manifold + contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j; + contactManifoldInfo.nbPotentialContactPoints++; - potentialContactPoints.add(contactPoint); + // Add the contact point to the array of potential contact points + const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; + + potentialContactPoints.add(contactPoint); + } } // Add the contact manifold to the overlapping pair contact assert(pairId == contactManifoldInfo.pairId); - pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex); + pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex; + pairContact->nbPotentialContactManifolds = 1; } else { @@ -1213,7 +1218,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na const uint32 newContactPairIndex = contactPairs->size(); contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, - newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator()); + newContactPairIndex, overlappingPair->collidingInPreviousFrame , isTrigger); pairContact = &((*contactPairs)[newContactPairIndex]); mapPairIdToContactPairIndex.add(Pair(pairId, newContactPairIndex)); @@ -1241,48 +1246,53 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na bool similarManifoldFound = false; // For each contact manifold of the overlapping pair - const uint32 nbPotentialManifolds = pairContact->potentialContactManifoldsIndices.size(); - for (uint32 m=0; m < nbPotentialManifolds; m++) { + for (uint32 m=0; m < pairContact->nbPotentialContactManifolds; m++) { uint32 contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m]; - // Get the first contact point of the current manifold - assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0); - const uint32 manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0]; - const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex]; + if (potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { - // 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) { + // 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]; - // Add the contact point to the manifold - potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex); + // 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) { - similarManifoldFound = true; + // Add the contact point to the manifold + potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex; + potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++; - break; - } + similarManifoldFound = true; + + break; + } + } } // If we have not found a manifold with a similar contact normal for the contact point - if (!similarManifoldFound) { + if (!similarManifoldFound && pairContact->nbPotentialContactManifolds < NB_MAX_POTENTIAL_CONTACT_MANIFOLDS) { // Create a new potential contact manifold for the overlapping pair uint32 contactManifoldIndex = potentialContactManifolds.size(); - potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator()); + potentialContactManifolds.emplace(pairId); ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; // Add the contact point to the manifold - contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex); + 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.add(contactManifoldIndex); + pairContact->potentialContactManifoldsIndices[pairContact->nbPotentialContactManifolds] = contactManifoldIndex; + pairContact->nbPotentialContactManifolds++; } - assert(pairContact->potentialContactManifoldsIndices.size() > 0); + assert(pairContact->nbPotentialContactManifolds > 0); } } @@ -1305,12 +1315,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array NB_MAX_CONTACT_MANIFOLDS) { + 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 (uint32 j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) { + for (uint32 j=0; j < contactPair.nbPotentialContactManifolds; j++) { ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]]; @@ -1335,12 +1345,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array MAX_CONTACT_POINTS_IN_MANIFOLD) { + if (manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(pairContact.collider1Entity); @@ -1348,25 +1358,20 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array& potentialContactPoints) const { + const Array& potentialContactPoints) const { decimal largestDepth = 0.0f; - const uint32 nbContactPoints = static_cast(manifold.potentialContactPointsIndices.size()); + assert(manifold.nbPotentialContactPoints > 0); - assert(nbContactPoints > 0); - - for (uint32 i=0; i < nbContactPoints; i++) { + for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) { decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth; if (depth > largestDepth) { @@ -1384,15 +1389,18 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, const Array& 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); // 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 - Array candidatePointsIndices(manifold.potentialContactPointsIndices); - assert(manifold.potentialContactPointsIndices.size() == candidatePointsIndices.size()); + 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; @@ -1416,7 +1424,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold const Vector3 searchDirection(1, 1, 1); decimal maxDotProduct = DECIMAL_SMALLEST; uint32 elementIndexToKeep = 0; - for (uint32 i=0; i < candidatePointsIndices.size(); i++) { + for (uint32 i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; decimal dotProduct = searchDirection.dot(element.localPoint1); @@ -1427,7 +1435,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep]; - removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); //candidatePointsIndices.removeAt(elementIndexToKeep); assert(nbReducedPoints == 1); @@ -1436,7 +1444,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal maxDistance = decimal(0.0); elementIndexToKeep = 0; - for (uint32 i=0; i < candidatePointsIndices.size(); i++) { + for (uint32 i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1452,7 +1460,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep]; - removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); assert(nbReducedPoints == 2); // Compute the third contact point we need to keep. @@ -1465,7 +1473,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal minArea = decimal(0.0); decimal maxArea = decimal(0.0); bool isPreviousAreaPositive = true; - for (uint32 i=0; i < candidatePointsIndices.size(); i++) { + for (uint32 i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1494,12 +1502,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold if (maxArea > (-minArea)) { isPreviousAreaPositive = true; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex]; - removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex); + removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints); } else { isPreviousAreaPositive = false; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex]; - removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex); + removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints); } nbReducedPoints = 3; @@ -1512,7 +1520,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal area; // For each remaining candidate points - for (uint32 i=0; i < candidatePointsIndices.size(); i++) { + for (uint32 i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; @@ -1549,51 +1557,22 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep]; - removeItemAtInArray(candidatePointsIndices, 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]); -} - -// 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--; - } - } - } + 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(Array& array, uint32 index) const { - assert(index < array.size()); - assert(array.size() > 0); - array.removeAtAndReplaceByLast(index); +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