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<uint> 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<uint> 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<ContactPointInfo>& potentialContactPoints) const;
+ /// Remove the duplicated contact points in a given contact manifold
+ void removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold,
+ const Array<ContactPointInfo>& potentialContactPoints) const;
+
/// Report contacts
void reportContacts(CollisionCallback& callback, Array<ContactPair>* contactPairs,
Array<ContactManifold>* manifolds, Array<ContactPoint>* contactPoints, Array<ContactPair>& lostContactPairs);
@@ -289,7 +293,7 @@ class CollisionDetectionSystem {
void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, Array<uint64>& convexPairs, Array<uint64>& 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<uint>& 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<int8>(potentialManifold.nbPotentialContactPoints);
+ const uint32 nbContactPoints = static_cast<uint32>(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<ContactPair>& 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<uint32>(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<uint>(potentialContactManifolds.size());
- potentialContactManifolds.emplace(pairId);
+ potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator());
ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
const uint32 contactPointIndexStart = static_cast<uint>(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<uint64, uint>(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<ContactPair
ContactPair& contactPair = (*contactPairs)[i];
// While there are too many manifolds in the contact pair
- while(contactPair.nbPotentialContactManifolds > 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<ContactPair
const ContactPair& pairContact = (*contactPairs)[i];
// For each potential contact manifold
- for (uint32 j=0; j < pairContact.nbPotentialContactManifolds; j++) {
+ for (uint32 j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) {
ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
// If there are two many contact points in the manifold
- if (manifold.nbPotentialContactPoints > 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<ContactPair
reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
}
- assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+ assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+ // Remove the duplicated contact points in the manifold (if any)
+ removeDuplicatedContactPointsInManifold(manifold, potentialContactPoints);
}
}
}
// Return the largest depth of all the contact points of a potential manifold
decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
- const Array<ContactPointInfo>& potentialContactPoints) const {
+ const Array<ContactPointInfo>& potentialContactPoints) const {
decimal largestDepth = 0.0f;
- assert(manifold.nbPotentialContactPoints > 0);
+ const uint32 nbContactPoints = static_cast<uint32>(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<ContactPointInfo>& 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<uint> 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<ContactPointInfo>& 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<uint>& array, uint32 index) const {
+ assert(index < array.size());
+ assert(array.size() > 0);
+ array.removeAtAndReplaceByLast(index);
}
// Report contacts and triggers