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

Merge branch 'contacts-fix' into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2021-07-26 20:30:20 +02:00
commit cc970840b1
6 changed files with 129 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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) {
}

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

@ -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);
}
};

7
include/reactphysics3d/configuration.h
View File

@ -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");

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

@ -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 :

6
src/engine/PhysicsWorld.cpp
View File

@ -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

189
src/systems/CollisionDetectionSystem.cpp
View File

@ -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