mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge conflicts when reverting commit a8f939c7

Browse Source
This commit is contained in:
Daniel Chappuis 2021-09-28 21:51:43 +02:00
parent f2723c6912
commit a0c2c9fe23
6 changed files with 107 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/reactphysics3d/collision/ContactManifoldInfo.h
View File

@ -45,11 +45,12 @@ struct ContactManifoldInfo {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Memory allocator /// Number of potential contact points
MemoryAllocator& allocator; uint8 nbPotentialContactPoints;
/// Indices of the contact points in the mPotentialContactPoints array /// Indices of the contact points in the mPotentialContactPoints array
Array<uint> potentialContactPointsIndices; uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
/// Overlapping pair id /// Overlapping pair id
uint64 pairId; uint64 pairId;
@ -57,8 +58,7 @@ struct ContactManifoldInfo {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator) ContactManifoldInfo(uint64 pairId) : nbPotentialContactPoints(0), pairId(pairId) {
: allocator(allocator), potentialContactPointsIndices(allocator, 4), pairId(pairId) {
} }

18
include/reactphysics3d/collision/ContactPair.h
View File

@ -44,14 +44,14 @@ struct ContactPair {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Reference to the memory allocator
MemoryAllocator& allocator;
/// Overlapping pair Id /// Overlapping pair Id
uint64 pairId; uint64 pairId;
/// Number of potential contact manifolds
uint8 nbPotentialContactManifolds;
/// Indices of the potential contact manifolds /// Indices of the potential contact manifolds
Array<uint32> potentialContactManifoldsIndices; uint32 potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS];
/// Entity of the first body of the contact /// Entity of the first body of the contact
Entity body1Entity; Entity body1Entity;
@ -93,9 +93,8 @@ struct ContactPair {
/// Constructor /// Constructor
ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity, ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity,
Entity collider2Entity, uint32 contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator) Entity collider2Entity, uint32 contactPairIndex, bool collidingInPreviousFrame, bool isTrigger)
: allocator(allocator), pairId(pairId), potentialContactManifoldsIndices(allocator, 1), : pairId(pairId), nbPotentialContactManifolds(0), body1Entity(body1Entity), body2Entity(body2Entity),
body1Entity(body1Entity), body2Entity(body2Entity),
collider1Entity(collider1Entity), collider2Entity(collider2Entity), collider1Entity(collider1Entity), collider2Entity(collider2Entity),
isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0), isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) { 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 // Remove a potential manifold at a given index in the array
void removePotentialManifoldAtIndex(uint32 index) { void removePotentialManifoldAtIndex(uint32 index) {
assert(index < potentialContactManifoldsIndices.size()); assert(index < nbPotentialContactManifolds);
potentialContactManifoldsIndices.removeAtAndReplaceByLast(index); potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1];
nbPotentialContactManifolds--;
} }
}; };

7
include/reactphysics3d/configuration.h
View File

@ -120,8 +120,11 @@ constexpr uint8 NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO = 16;
/// Maximum number of contact manifolds in an overlapping pair /// Maximum number of contact manifolds in an overlapping pair
constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3; constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3;
/// Distance threshold to consider that two contact points in a manifold are the same /// Maximum number of potential contact manifolds in an overlapping pair
constexpr decimal SAME_CONTACT_POINT_DISTANCE_THRESHOLD = decimal(0.01); 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 /// Current version of ReactPhysics3D
const std::string RP3D_VERSION = std::string("0.8.0"); const std::string RP3D_VERSION = std::string("0.8.0");

6
include/reactphysics3d/systems/CollisionDetectionSystem.h
View File

@ -265,10 +265,6 @@ class CollisionDetectionSystem {
void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
const Array<ContactPointInfo>& potentialContactPoints) const; 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 /// Report contacts
void reportContacts(CollisionCallback& callback, Array<ContactPair>* contactPairs, void reportContacts(CollisionCallback& callback, Array<ContactPair>* contactPairs,
Array<ContactManifold>* manifolds, Array<ContactPoint>* contactPoints, Array<ContactPair>& lostContactPairs); Array<ContactManifold>* manifolds, Array<ContactPoint>* contactPoints, Array<ContactPair>& lostContactPairs);
@ -293,7 +289,7 @@ class CollisionDetectionSystem {
void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, Array<uint64>& convexPairs, Array<uint64>& concavePairs) const; 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) /// Remove an element in an array (and replace it by the last one in the array)
void removeItemAtInArray(Array<uint>& array, uint32 index) const; void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const;
public : public :

6
src/engine/PhysicsWorld.cpp
View File

@ -850,11 +850,11 @@ void PhysicsWorld::createIslands() {
mProcessContactPairsOrderIslands.add(contactPairIndex); mProcessContactPairsOrderIslands.add(contactPairIndex);
assert(pair.potentialContactManifoldsIndices.size() > 0); assert(pair.nbPotentialContactManifolds > 0);
nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); nbTotalManifolds += pair.nbPotentialContactManifolds;
// Add the contact manifold into the island // Add the contact manifold into the island
mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); mIslands.nbContactManifolds[islandIndex] += pair.nbPotentialContactManifolds;
pair.isAlreadyInIsland = true; pair.isAlreadyInIsland = true;
// Check if the other body has already been added to the island // Check if the other body has already been added to the island

189
src/systems/CollisionDetectionSystem.cpp
View File

@ -192,7 +192,7 @@ void CollisionDetectionSystem::addLostContactPair(OverlappingPairs::OverlappingP
// Create a lost contact pair // Create a lost contact pair
ContactPair lostContactPair(overlappingPair.pairID, body1Entity, body2Entity, overlappingPair.collider1, overlappingPair.collider2, mLostContactPairs.size(), ContactPair lostContactPair(overlappingPair.pairID, body1Entity, body2Entity, overlappingPair.collider1, overlappingPair.collider2, mLostContactPairs.size(),
true, isTrigger, mMemoryManager.getHeapAllocator()); true, isTrigger);
mLostContactPairs.add(lostContactPair); mLostContactPairs.add(lostContactPair);
} }
@ -742,7 +742,7 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf
const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
// Create a new contact pair // 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); contactPairs.add(contactPair);
setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId); setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId);
@ -840,27 +840,27 @@ void CollisionDetectionSystem::createContacts() {
ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex]; ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
contactPair.contactManifoldsIndex = mCurrentContactManifolds->size(); contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
contactPair.contactPointsIndex = mCurrentContactPoints->size(); contactPair.contactPointsIndex = mCurrentContactPoints->size();
// For each potential contact manifold of the pair // 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]]; ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
// Start index and number of contact points for this manifold // Start index and number of contact points for this manifold
const uint32 contactPointsIndex = mCurrentContactPoints->size(); const uint32 contactPointsIndex = mCurrentContactPoints->size();
const uint32 nbContactPoints = static_cast<uint32>(potentialManifold.potentialContactPointsIndices.size()); const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints);
contactPair.nbToTalContactPoints += nbContactPoints; contactPair.nbToTalContactPoints += nbContactPoints;
// Create and add the contact manifold // Create and add the contact manifold
mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
contactPair.collider2Entity, contactPointsIndex, nbContactPoints); contactPair.collider2Entity, contactPointsIndex, nbContactPoints);
assert(potentialManifold.potentialContactPointsIndices.size() > 0); assert(potentialManifold.nbPotentialContactPoints > 0);
// For each contact point of the manifold // 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]]; ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
@ -948,30 +948,30 @@ void CollisionDetectionSystem::createSnapshotContacts(Array<ContactPair>& contac
for (uint32 p=0; p < nbContactPairs; p++) { for (uint32 p=0; p < nbContactPairs; p++) {
ContactPair& contactPair = contactPairs[p]; ContactPair& contactPair = contactPairs[p];
assert(contactPair.potentialContactManifoldsIndices.size() > 0); assert(contactPair.nbPotentialContactManifolds > 0);
contactPair.contactManifoldsIndex = contactManifolds.size(); contactPair.contactManifoldsIndex = contactManifolds.size();
contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
contactPair.contactPointsIndex = contactPoints.size(); contactPair.contactPointsIndex = contactPoints.size();
// For each potential contact manifold of the pair // 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]]; ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
// Start index and number of contact points for this manifold // Start index and number of contact points for this manifold
const uint32 contactPointsIndex = contactPoints.size(); const uint32 contactPointsIndex = contactPoints.size();
const uint32 nbContactPoints = static_cast<uint32>(potentialManifold.potentialContactPointsIndices.size()); const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints;
contactPair.nbToTalContactPoints += nbContactPoints; contactPair.nbToTalContactPoints += nbContactPoints;
// Create and add the contact manifold // Create and add the contact manifold
contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
contactPair.collider2Entity, contactPointsIndex, nbContactPoints); contactPair.collider2Entity, contactPointsIndex, nbContactPoints);
assert(nbContactPoints > 0); assert(potentialManifold.nbPotentialContactPoints > 0);
// For each contact point of the manifold // 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]]; ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
@ -1171,13 +1171,13 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
const uint32 newContactPairIndex = contactPairs->size(); const uint32 newContactPairIndex = contactPairs->size();
contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator()); newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger);
ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]); ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]);
// Create a new potential contact manifold for the overlapping pair // Create a new potential contact manifold for the overlapping pair
uint32 contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size()); uint32 contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator()); potentialContactManifolds.emplace(pairId);
ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
const uint32 contactPointIndexStart = static_cast<uint>(potentialContactPoints.size()); const uint32 contactPointIndexStart = static_cast<uint>(potentialContactPoints.size());
@ -1185,18 +1185,23 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
// Add the potential contacts // Add the potential contacts
for (uint32 j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { for (uint32 j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) {
// Add the contact point to the manifold if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndexStart + j);
// Add the contact point to the array of potential contact points // Add the contact point to the manifold
const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; 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 // Add the contact manifold to the overlapping pair contact
assert(pairId == contactManifoldInfo.pairId); assert(pairId == contactManifoldInfo.pairId);
pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex); pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex;
pairContact->nbPotentialContactManifolds = 1;
} }
else { else {
@ -1213,7 +1218,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
const uint32 newContactPairIndex = contactPairs->size(); const uint32 newContactPairIndex = contactPairs->size();
contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator()); newContactPairIndex, overlappingPair->collidingInPreviousFrame , isTrigger);
pairContact = &((*contactPairs)[newContactPairIndex]); pairContact = &((*contactPairs)[newContactPairIndex]);
mapPairIdToContactPairIndex.add(Pair<uint64, uint>(pairId, newContactPairIndex)); mapPairIdToContactPairIndex.add(Pair<uint64, uint>(pairId, newContactPairIndex));
@ -1241,48 +1246,53 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
bool similarManifoldFound = false; bool similarManifoldFound = false;
// For each contact manifold of the overlapping pair // For each contact manifold of the overlapping pair
const uint32 nbPotentialManifolds = pairContact->potentialContactManifoldsIndices.size(); for (uint32 m=0; m < pairContact->nbPotentialContactManifolds; m++) {
for (uint32 m=0; m < nbPotentialManifolds; m++) {
uint32 contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m]; uint32 contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
// Get the first contact point of the current manifold if (potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0);
const uint32 manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex];
// If we have found a corresponding manifold for the new contact point // Get the first contact point of the current manifold
// (a manifold with a similar contact normal direction) assert(potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints > 0);
if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) { const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex];
// Add the contact point to the manifold // If we have found a corresponding manifold for the new contact point
potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex); // (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 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 // Create a new potential contact manifold for the overlapping pair
uint32 contactManifoldIndex = potentialContactManifolds.size(); uint32 contactManifoldIndex = potentialContactManifolds.size();
potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator()); potentialContactManifolds.emplace(pairId);
ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
// Add the contact point to the manifold // Add the contact point to the manifold
contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex); contactManifoldInfo.potentialContactPointsIndices[0] = contactPointIndex;
contactManifoldInfo.nbPotentialContactPoints = 1;
assert(pairContact != nullptr); assert(pairContact != nullptr);
// Add the contact manifold to the overlapping pair contact // Add the contact manifold to the overlapping pair contact
assert(pairContact->pairId == contactManifoldInfo.pairId); 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<ContactPair
ContactPair& contactPair = (*contactPairs)[i]; ContactPair& contactPair = (*contactPairs)[i];
// While there are too many manifolds in the contact pair // While there are too many manifolds in the contact pair
while(contactPair.potentialContactManifoldsIndices.size() > NB_MAX_CONTACT_MANIFOLDS) { while(contactPair.nbPotentialContactManifolds > NB_MAX_CONTACT_MANIFOLDS) {
// Look for a manifold with the smallest contact penetration depth. // Look for a manifold with the smallest contact penetration depth.
decimal minDepth = DECIMAL_LARGEST; decimal minDepth = DECIMAL_LARGEST;
int minDepthManifoldIndex = -1; 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]]; ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]];
@ -1335,12 +1345,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array<ContactPair
const ContactPair& pairContact = (*contactPairs)[i]; const ContactPair& pairContact = (*contactPairs)[i];
// For each potential contact manifold // For each potential contact manifold
for (uint32 j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) { for (uint32 j=0; j < pairContact.nbPotentialContactManifolds; j++) {
ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]]; ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
// If there are two many contact points in the manifold // 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) {
Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(pairContact.collider1Entity); Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(pairContact.collider1Entity);
@ -1348,25 +1358,20 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array<ContactPair
reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints); reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
} }
assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD); assert(manifold.nbPotentialContactPoints <= 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 // Return the largest depth of all the contact points of a potential manifold
decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold, decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
const Array<ContactPointInfo>& potentialContactPoints) const { const Array<ContactPointInfo>& potentialContactPoints) const {
decimal largestDepth = 0.0f; decimal largestDepth = 0.0f;
const uint32 nbContactPoints = static_cast<uint32>(manifold.potentialContactPointsIndices.size()); assert(manifold.nbPotentialContactPoints > 0);
assert(nbContactPoints > 0); for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) {
for (uint32 i=0; i < nbContactPoints; i++) {
decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth; decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth;
if (depth > largestDepth) { if (depth > largestDepth) {
@ -1384,15 +1389,18 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co
void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
const Array<ContactPointInfo>& potentialContactPoints) const { const Array<ContactPointInfo>& 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 // The following algorithm only works to reduce to a maximum of 4 contact points
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
// Array of the candidate contact points indices in the manifold. Every time that we have found a // 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 // point we want to keep, we will remove it from this array
Array<uint> candidatePointsIndices(manifold.potentialContactPointsIndices); uint candidatePointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
assert(manifold.potentialContactPointsIndices.size() == candidatePointsIndices.size()); uint8 nbCandidatePoints = manifold.nbPotentialContactPoints;
for (uint8 i=0 ; i < manifold.nbPotentialContactPoints; i++) {
candidatePointsIndices[i] = manifold.potentialContactPointsIndices[i];
}
int8 nbReducedPoints = 0; int8 nbReducedPoints = 0;
@ -1416,7 +1424,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
const Vector3 searchDirection(1, 1, 1); const Vector3 searchDirection(1, 1, 1);
decimal maxDotProduct = DECIMAL_SMALLEST; decimal maxDotProduct = DECIMAL_SMALLEST;
uint32 elementIndexToKeep = 0; uint32 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& element = potentialContactPoints[candidatePointsIndices[i]];
decimal dotProduct = searchDirection.dot(element.localPoint1); decimal dotProduct = searchDirection.dot(element.localPoint1);
@ -1427,7 +1435,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
} }
} }
pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep]; pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep];
removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
//candidatePointsIndices.removeAt(elementIndexToKeep); //candidatePointsIndices.removeAt(elementIndexToKeep);
assert(nbReducedPoints == 1); assert(nbReducedPoints == 1);
@ -1436,7 +1444,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
decimal maxDistance = decimal(0.0); decimal maxDistance = decimal(0.0);
elementIndexToKeep = 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& element = potentialContactPoints[candidatePointsIndices[i]];
const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@ -1452,7 +1460,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
} }
pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep]; pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep];
removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
assert(nbReducedPoints == 2); assert(nbReducedPoints == 2);
// Compute the third contact point we need to keep. // Compute the third contact point we need to keep.
@ -1465,7 +1473,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
decimal minArea = decimal(0.0); decimal minArea = decimal(0.0);
decimal maxArea = decimal(0.0); decimal maxArea = decimal(0.0);
bool isPreviousAreaPositive = true; 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& element = potentialContactPoints[candidatePointsIndices[i]];
const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@ -1494,12 +1502,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
if (maxArea > (-minArea)) { if (maxArea > (-minArea)) {
isPreviousAreaPositive = true; isPreviousAreaPositive = true;
pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex]; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex];
removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex); removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints);
} }
else { else {
isPreviousAreaPositive = false; isPreviousAreaPositive = false;
pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex]; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex];
removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex); removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints);
} }
nbReducedPoints = 3; nbReducedPoints = 3;
@ -1512,7 +1520,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
decimal area; decimal area;
// For each remaining candidate points // 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]]; const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
@ -1549,51 +1557,22 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
} }
} }
pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep]; pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
removeItemAtInArray(candidatePointsIndices, elementIndexToKeep); removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
// Only keep the four selected contact points in the manifold // Only keep the four selected contact points in the manifold
manifold.potentialContactPointsIndices.clear(); manifold.potentialContactPointsIndices[0] = pointsToKeepIndices[0];
manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]); manifold.potentialContactPointsIndices[1] = pointsToKeepIndices[1];
manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]); manifold.potentialContactPointsIndices[2] = pointsToKeepIndices[2];
manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]); manifold.potentialContactPointsIndices[3] = pointsToKeepIndices[3];
manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]); manifold.nbPotentialContactPoints = 4;
}
// 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) // Remove an element in an array (and replace it by the last one in the array)
void CollisionDetectionSystem::removeItemAtInArray(Array<uint>& array, uint32 index) const { void CollisionDetectionSystem::removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const {
assert(index < array.size()); assert(index < arraySize);
assert(array.size() > 0); assert(arraySize > 0);
array.removeAtAndReplaceByLast(index); array[index] = array[arraySize - 1];
arraySize--;
} }
// Report contacts and triggers // Report contacts and triggers