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

Working on optimizations in contacts creation

Browse Source
This commit is contained in:
Daniel Chappuis 2020-07-18 17:03:44 +02:00
parent b410b26e23
commit 0032fef473
11 changed files with 258 additions and 184 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
include/reactphysics3d/collision/ContactManifold.h
View File

@ -58,7 +58,7 @@ class ContactManifold {
// -------------------- Constants -------------------- //
/// Maximum number of contact points in a reduced contact manifold
const int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
static constexpr int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
// -------------------- Attributes -------------------- //
@ -78,7 +78,7 @@ class ContactManifold {
Entity colliderEntity2;
/// Number of contacts in the cache
int8 nbContactPoints;
uint8 nbContactPoints;
/// First friction vector of the contact manifold
Vector3 frictionVector1;
@ -107,16 +107,7 @@ class ContactManifold {
/// Constructor
ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
uint contactPointsIndex, int8 nbContactPoints);
/// Destructor
~ContactManifold();
/// Copy-constructor
ContactManifold(const ContactManifold& contactManifold) = default;
/// Assignment operator
ContactManifold& operator=(const ContactManifold& contactManifold) = default;
uint contactPointsIndex, uint8 nbContactPoints);
// -------------------- Friendship -------------------- //

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

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

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

@ -47,8 +47,11 @@ struct ContactPair {
/// Overlapping pair Id
uint64 pairId;
/// Number of potential contact manifolds
uint8 nbPotentialContactManifolds;
/// Indices of the potential contact manifolds
List<uint> potentialContactManifoldsIndices;
uint potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS];
/// Entity of the first body of the contact
Entity body1Entity;
@ -90,13 +93,21 @@ struct ContactPair {
/// Constructor
ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity,
Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator)
: pairId(pairId), potentialContactManifoldsIndices(allocator, 8), body1Entity(body1Entity), body2Entity(body2Entity),
Entity collider2Entity, uint 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) {
}
// Remove a potential manifold at a given index in the array
void removePotentialManifoldAtIndex(uint index) {
assert(index < nbPotentialContactManifolds);
potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1];
nbPotentialContactManifolds--;
}
};
}

3
include/reactphysics3d/collision/ContactPointInfo.h
View File

@ -76,9 +76,6 @@ struct ContactPointInfo {
assert(contactNormal.lengthSquare() > decimal(0.8));
assert(penDepth > decimal(0.0));
}
/// Destructor
~ContactPointInfo() = default;
};
}

9
include/reactphysics3d/configuration.h
View File

@ -103,6 +103,15 @@ constexpr decimal PI_TIMES_2 = decimal(6.28318530);
/// without triggering a large modification of the tree each frame which can be costly
constexpr decimal DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE = decimal(0.08);
/// 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 = 12;
/// Current version of ReactPhysics3D
const std::string RP3D_VERSION = std::string("0.8.0");

15
include/reactphysics3d/containers/List.h
View File

@ -294,6 +294,21 @@ class List {
mSize++;
}
/// Add an element into the list by constructing it directly into the list (in order to avoid a copy)
template<typename...Ts>
void emplace(Ts&&... args) {
// If we need to allocate more memory
if (mSize == mCapacity) {
reserve(mCapacity == 0 ? 1 : mCapacity * 2);
}
// Construct the element directly at its location in the array
new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(std::forward<Ts>(args)...);
mSize++;
}
/// Add a given numbers of elements at the end of the list but do not init them
void addWithoutInit(uint nbElements) {

5
include/reactphysics3d/engine/PhysicsWorld.h
View File

@ -116,9 +116,6 @@ class PhysicsWorld {
/// might enter sleeping mode
decimal defaultSleepAngularVelocity;
/// Maximum number of contact manifolds in an overlapping pair
uint nbMaxContactManifolds;
/// This is used to test if two contact manifold are similar (same contact normal) in order to
/// merge them. If the cosine of the angle between the normals of the two manifold are larger
/// than the value bellow, the manifold are considered to be similar.
@ -139,7 +136,6 @@ class PhysicsWorld {
defaultTimeBeforeSleep = 1.0f;
defaultSleepLinearVelocity = decimal(0.02);
defaultSleepAngularVelocity = decimal(3.0) * (PI_RP3D / decimal(180.0));
nbMaxContactManifolds = 3;
cosAngleSimilarContactManifold = decimal(0.95);
}
@ -163,7 +159,6 @@ class PhysicsWorld {
ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl;
ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl;
ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl;
ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl;
ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl;
return ss.str();

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

@ -128,19 +128,9 @@ class CollisionDetectionSystem {
/// List of lost contact pairs (contact pairs in contact in previous frame but not in the current one)
List<ContactPair> mLostContactPairs;
/// First map of overlapping pair id to the index of the corresponding pair contact
Map<uint64, uint> mMapPairIdToContactPairIndex1;
/// Second map of overlapping pair id to the index of the corresponding pair contact
Map<uint64, uint> mMapPairIdToContactPairIndex2;
/// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame
/// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
Map<uint64, uint>* mPreviousMapPairIdToContactPairIndex;
/// Pointer to the map of overlappingPairId to the index of contact pair of the current frame
/// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
Map<uint64, uint>* mCurrentMapPairIdToContactPairIndex;
Map<uint64, uint> mPreviousMapPairIdToContactPairIndex;
/// First list with the contact manifolds
List<ContactManifold> mContactManifolds1;
@ -223,12 +213,11 @@ class CollisionDetectionSystem {
/// Convert the potential contact into actual contacts
void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
Map<uint64, uint>* mapPairIdToContactPairIndex,
List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs);
List<ContactManifoldInfo>& potentialContactManifolds,
Map<uint64, uint>& mapPairIdToContactPairIndex, List<ContactPair>* contactPairs);
/// Process the potential contacts after narrow-phase collision detection
void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
Map<uint64, uint>* mapPairIdToContactPairIndex,
List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs);
/// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
@ -238,6 +227,9 @@ class CollisionDetectionSystem {
/// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
void createContacts();
/// Compute the map from contact pairs ids to contact pair for the next frame
void computeMapPreviousContactPairs();
/// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)
void computeLostContactPairs();
@ -277,6 +269,9 @@ class CollisionDetectionSystem {
/// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List<uint64>& convexPairs, List<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;
public :
// -------------------- Methods -------------------- //

7
src/collision/ContactManifold.cpp
View File

@ -32,14 +32,9 @@ using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
uint contactPointsIndex, int8 nbContactPoints)
uint contactPointsIndex, uint8 nbContactPoints)
:contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0),
frictionTwistImpulse(0), isAlreadyInIsland(false) {
}
// Destructor
ContactManifold::~ContactManifold() {
}

6
src/engine/PhysicsWorld.cpp
View File

@ -824,11 +824,11 @@ void PhysicsWorld::createIslands() {
if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex)
&& !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) {
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

333
src/systems/CollisionDetectionSystem.cpp
View File

@ -62,13 +62,11 @@ CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, Collider
mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()),
mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()),
mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()), mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1),
mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2), mContactManifolds1(mMemoryManager.getPoolAllocator()),
mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
mContactPoints1(mMemoryManager.getPoolAllocator()),
mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
mCurrentContactPoints(&mContactPoints2) {
mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mPreviousMapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator()),
mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()),
mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
mContactPoints1(mMemoryManager.getPoolAllocator()), mContactPoints2(mMemoryManager.getPoolAllocator()),
mPreviousContactPoints(&mContactPoints1), mCurrentContactPoints(&mContactPoints2) {
#ifdef IS_RP3D_PROFILING_ENABLED
@ -159,7 +157,7 @@ void CollisionDetectionSystem::addLostContactPair(uint64 overlappingPairIndex) {
// Create a lost contact pair
ContactPair lostContactPair(mOverlappingPairs.mPairIds[overlappingPairIndex], body1Entity, body2Entity, collider1Entity, collider2Entity, mLostContactPairs.size(),
true, isTrigger, mMemoryManager.getHeapAllocator());
true, isTrigger);
mLostContactPairs.add(lostContactPair);
}
@ -480,13 +478,14 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow
// Process the potential contacts after narrow-phase collision detection
void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo,
List<ContactPointInfo>& potentialContactPoints,
Map<uint64, uint>* mapPairIdToContactPairIndex,
List<ContactManifoldInfo>& potentialContactManifolds,
List<ContactPair>* contactPairs) {
assert(contactPairs->size() == 0);
assert(mapPairIdToContactPairIndex->size() == 0);
Map<uint64, uint> mapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator(), mPreviousMapPairIdToContactPairIndex.size());
// get the narrow-phase batches to test for collision
NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch();
NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch();
@ -496,18 +495,13 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar
NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch();
// Process the potential contacts
processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
potentialContactManifolds, contactPairs);
processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
potentialContactManifolds, contactPairs);
processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
potentialContactManifolds, contactPairs);
processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
potentialContactManifolds, contactPairs);
processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
potentialContactManifolds, contactPairs);
processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
potentialContactManifolds, contactPairs);
processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints,
potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
}
// Compute the narrow-phase collision detection
@ -527,7 +521,7 @@ void CollisionDetectionSystem::computeNarrowPhase() {
testNarrowPhaseCollision(mNarrowPhaseInput, true, allocator);
// Process all the potential contacts after narrow-phase collision
processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex,
processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints,
mPotentialContactManifolds, mCurrentContactPairs);
// Reduce the number of contact points in the manifolds
@ -539,9 +533,21 @@ void CollisionDetectionSystem::computeNarrowPhase() {
// Create the actual narrow-phase contacts
createContacts();
// Compute the map from contact pairs ids to contact pair for the next frame
computeMapPreviousContactPairs();
mNarrowPhaseInput.clear();
}
/// Compute the map from contact pairs ids to contact pair for the next frame
void CollisionDetectionSystem::computeMapPreviousContactPairs() {
mPreviousMapPairIdToContactPairIndex.clear();
for (uint i=0; i < mCurrentContactPairs->size(); i++) {
mPreviousMapPairIdToContactPairIndex.add(Pair<uint64, uint>((*mCurrentContactPairs)[i].pairId, i));
}
}
// Compute the narrow-phase collision detection for the testOverlap() methods.
/// This method returns true if contacts are found.
bool CollisionDetectionSystem::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& narrowPhaseInput, OverlapCallback* callback) {
@ -629,8 +635,7 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf
const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
// Create a new contact pair
ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity,
contactPairs.size(), isTrigger, false, mMemoryManager.getHeapAllocator());
ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false);
contactPairs.add(contactPair);
setOverlapContactPairId.add(narrowPhaseInfoBatch.overlappingPairIds[i]);
@ -657,14 +662,13 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn
List<ContactPointInfo> potentialContactPoints(allocator);
List<ContactManifoldInfo> potentialContactManifolds(allocator);
Map<uint64, uint> mapPairIdToContactPairIndex(allocator);
List<ContactPair> contactPairs(allocator);
List<ContactPair> lostContactPairs(allocator); // Not used during collision snapshots
List<ContactManifold> contactManifolds(allocator);
List<ContactPoint> contactPoints(allocator);
// Process all the potential contacts after narrow-phase collision
processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, &contactPairs);
processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, potentialContactManifolds, &contactPairs);
// Reduce the number of contact points in the manifolds
reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints);
@ -687,24 +691,20 @@ void CollisionDetectionSystem::swapPreviousAndCurrentContacts() {
mPreviousContactPairs = &mContactPairs2;
mPreviousContactManifolds = &mContactManifolds2;
mPreviousContactPoints = &mContactPoints2;
mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2;
mCurrentContactPairs = &mContactPairs1;
mCurrentContactManifolds = &mContactManifolds1;
mCurrentContactPoints = &mContactPoints1;
mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1;
}
else {
mPreviousContactPairs = &mContactPairs1;
mPreviousContactManifolds = &mContactManifolds1;
mPreviousContactPoints = &mContactPoints1;
mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1;
mCurrentContactPairs = &mContactPairs2;
mCurrentContactManifolds = &mContactManifolds2;
mCurrentContactPoints = &mContactPoints2;
mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2;
}
}
@ -722,7 +722,7 @@ void CollisionDetectionSystem::createContacts() {
ContactPair& contactPair = (*mCurrentContactPairs)[p];
contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
contactPair.contactPointsIndex = mCurrentContactPoints->size();
// Add the associated contact pair to both bodies of the pair (used to create islands later)
@ -734,13 +734,13 @@ void CollisionDetectionSystem::createContacts() {
}
// For each potential contact manifold of the pair
for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) {
ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
// Start index and number of contact points for this manifold
const uint contactPointsIndex = mCurrentContactPoints->size();
const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints);
contactPair.nbToTalContactPoints += nbContactPoints;
// We create a new contact manifold
@ -750,10 +750,10 @@ void CollisionDetectionSystem::createContacts() {
// Add the contact manifold
mCurrentContactManifolds->add(contactManifold);
assert(potentialManifold.potentialContactPointsIndices.size() > 0);
assert(potentialManifold.nbPotentialContactPoints > 0);
// For each contact point of the manifold
for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) {
ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
@ -775,7 +775,6 @@ void CollisionDetectionSystem::createContacts() {
mPreviousContactPoints->clear();
mPreviousContactManifolds->clear();
mPreviousContactPairs->clear();
mPreviousMapPairIdToContactPairIndex->clear();
// Reset the potential contacts
mPotentialContactPoints.clear(true);
@ -817,20 +816,20 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
for (uint p=0; p < contactPairs.size(); 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 (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) {
ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
// Start index and number of contact points for this manifold
const uint contactPointsIndex = contactPoints.size();
const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints;
contactPair.nbToTalContactPoints += nbContactPoints;
// We create a new contact manifold
@ -840,10 +839,10 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
// Add the contact manifold
contactManifolds.add(contactManifold);
assert(potentialManifold.potentialContactPointsIndices.size() > 0);
assert(potentialManifold.nbPotentialContactPoints > 0);
// For each contact point of the manifold
for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) {
ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
@ -866,10 +865,10 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() {
ContactPair& currentContactPair = (*mCurrentContactPairs)[i];
// Find the corresponding contact pair in the previous frame (if any)
auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex->find(currentContactPair.pairId);
auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex.find(currentContactPair.pairId);
// If we have found a corresponding contact pair in the previous frame
if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex->end()) {
if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex.end()) {
const uint previousContactPairIndex = itPrevContactPair->second;
ContactPair& previousContactPair = (*mPreviousContactPairs)[previousContactPairIndex];
@ -988,8 +987,8 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback,
// Convert the potential contact into actual contacts
void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo,
List<ContactPointInfo>& potentialContactPoints,
Map<uint64, uint>* mapPairIdToContactPairIndex,
List<ContactManifoldInfo>& potentialContactManifolds,
Map<uint64, uint>& mapPairIdToContactPairIndex,
List<ContactPair>* contactPairs) {
RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler);
@ -1012,100 +1011,151 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
mOverlappingPairs.mCollidingInCurrentFrame[pairIndex] = true;
// If there is not already a contact pair for this overlapping pair
auto it = mapPairIdToContactPairIndex->find(pairId);
ContactPair* pairContact = nullptr;
if (it == mapPairIdToContactPairIndex->end()) {
const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i];
const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i];
const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity);
const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity);
const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index];
const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index];
// If we have a convex vs convex collision (if we consider the base collision shapes of the colliders)
if (mCollidersComponents.mCollisionShapes[collider1Index]->isConvex() &&
mCollidersComponents.mCollisionShapes[collider2Index]->isConvex()) {
// Create a new ContactPair
const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i];
const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i];
const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity);
const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity);
const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index];
const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index];
const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity));
const uint newContactPairIndex = contactPairs->size();
ContactPair overlappingPairContact(pairId, body1Entity, body2Entity,
collider1Entity, collider2Entity,
newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger, mMemoryManager.getHeapAllocator());
contactPairs->add(overlappingPairContact);
pairContact = &((*contactPairs)[newContactPairIndex]);
mapPairIdToContactPairIndex->add(Pair<uint64, uint>(pairId, newContactPairIndex));
}
else { // If a ContactPair already exists for this overlapping pair, we use this one
assert(it->first == pairId);
contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger);
const uint pairContactIndex = it->second;
pairContact = &((*contactPairs)[pairContactIndex]);
}
ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]);
assert(pairContact != nullptr);
// Create a new potential contact manifold for the overlapping pair
uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
potentialContactManifolds.emplace(pairId);
ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
// Add the potential contacts
for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
const uint contactPointIndexStart = static_cast<uint>(potentialContactPoints.size());
const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
// Add the potential contacts
for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
// Add the contact point to the list of potential contact points
const uint contactPointIndex = static_cast<uint>(potentialContactPoints.size());
potentialContactPoints.add(contactPoint);
bool similarManifoldFound = false;
// For each contact manifold of the overlapping pair
for (uint m=0; m < pairContact->potentialContactManifoldsIndices.size(); m++) {
uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
// 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];
// 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 (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
// Add the contact point to the manifold
potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex);
contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j;
contactManifoldInfo.nbPotentialContactPoints++;
similarManifoldFound = true;
// Add the contact point to the list of potential contact points
const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
break;
potentialContactPoints.add(contactPoint);
}
}
// If we have not found a manifold with a similar contact normal for the contact point
if (!similarManifoldFound) {
// Add the contact manifold to the overlapping pair contact
assert(overlappingPairContact.pairId == contactManifoldInfo.pairId);
pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex;
pairContact->nbPotentialContactManifolds = 1;
}
else {
// Create a new contact manifold for the overlapping pair
ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator());
// If there is not already a contact pair for this overlapping pair
auto it = mapPairIdToContactPairIndex.find(pairId);
ContactPair* pairContact = nullptr;
if (it == mapPairIdToContactPairIndex.end()) {
// Add the contact point to the manifold
contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex);
// Create a new ContactPair
assert(pairContact != nullptr);
const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
// Add the potential contact manifold
uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
potentialContactManifolds.add(contactManifoldInfo);
assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity));
// Add the contact manifold to the overlapping pair contact
assert(pairContact->pairId == contactManifoldInfo.pairId);
pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
const uint newContactPairIndex = contactPairs->size();
contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger);
pairContact = &((*contactPairs)[newContactPairIndex]);
mapPairIdToContactPairIndex.add(Pair<uint64, uint>(pairId, newContactPairIndex));
}
else { // If a ContactPair already exists for this overlapping pair, we use this one
assert(it->first == pairId);
const uint pairContactIndex = it->second;
pairContact = &((*contactPairs)[pairContactIndex]);
}
assert(pairContact->potentialContactManifoldsIndices.size() > 0);
assert(pairContact != nullptr);
// Add the potential contacts
for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
// Add the contact point to the list of potential contact points
const uint contactPointIndex = static_cast<uint>(potentialContactPoints.size());
potentialContactPoints.add(contactPoint);
bool similarManifoldFound = false;
// For each contact manifold of the overlapping pair
for (uint m=0; m < pairContact->nbPotentialContactManifolds; 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].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) {
// Add the contact point to the manifold
potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex;
potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++;
similarManifoldFound = true;
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) {
// Create a new potential contact manifold for the overlapping pair
uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
potentialContactManifolds.emplace(pairId);
ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
// Add the contact point to the manifold
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[pairContact->nbPotentialContactManifolds] = contactManifoldIndex;
pairContact->nbPotentialContactManifolds++;
}
assert(pairContact->nbPotentialContactManifolds > 0);
}
}
narrowPhaseInfoBatch.resetContactPoints(i);
@ -1126,12 +1176,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
ContactPair& contactPair = (*contactPairs)[i];
// While there are too many manifolds in the contact pair
while(contactPair.potentialContactManifoldsIndices.size() > mWorld->mConfig.nbMaxContactManifolds) {
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 (uint j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) {
for (uint j=0; j < contactPair.nbPotentialContactManifolds; j++) {
ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]];
@ -1146,7 +1196,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
// Remove the non optimal manifold
assert(minDepthManifoldIndex >= 0);
contactPair.potentialContactManifoldsIndices.removeAt(minDepthManifoldIndex);
contactPair.removePotentialManifoldAtIndex(minDepthManifoldIndex);
}
}
@ -1156,12 +1206,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
const ContactPair& pairContact = (*contactPairs)[i];
// For each potential contact manifold
for (uint j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) {
for (uint j=0; j < pairContact.nbPotentialContactManifolds; j++) {
ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
// 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) {
Entity collider1 = mOverlappingPairs.mColliders1[mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]];
@ -1171,7 +1221,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
}
assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD);
assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
}
}
}
@ -1182,9 +1232,9 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co
decimal largestDepth = 0.0f;
assert(manifold.potentialContactPointsIndices.size() > 0);
assert(manifold.nbPotentialContactPoints > 0);
for (uint i=0; i < manifold.potentialContactPointsIndices.size(); i++) {
for (uint i=0; i < manifold.nbPotentialContactPoints; i++) {
decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth;
if (depth > largestDepth) {
@ -1202,14 +1252,18 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co
void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
const List<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
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
// List 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 list
List<uint> candidatePointsIndices(manifold.potentialContactPointsIndices);
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;
@ -1233,7 +1287,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
const Vector3 searchDirection(1, 1, 1);
decimal maxDotProduct = DECIMAL_SMALLEST;
uint elementIndexToKeep = 0;
for (uint i=0; i < candidatePointsIndices.size(); i++) {
for (uint i=0; i < nbCandidatePoints; i++) {
const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
decimal dotProduct = searchDirection.dot(element.localPoint1);
@ -1244,7 +1298,8 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
}
}
pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep];
candidatePointsIndices.removeAt(elementIndexToKeep);
removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
//candidatePointsIndices.removeAt(elementIndexToKeep);
assert(nbReducedPoints == 1);
// Compute the second contact point we need to keep.
@ -1252,7 +1307,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
decimal maxDistance = decimal(0.0);
elementIndexToKeep = 0;
for (uint i=0; i < candidatePointsIndices.size(); i++) {
for (uint i=0; i < nbCandidatePoints; i++) {
const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@ -1268,7 +1323,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
}
pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep];
candidatePointsIndices.removeAt(elementIndexToKeep);
removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
assert(nbReducedPoints == 2);
// Compute the third contact point we need to keep.
@ -1281,7 +1336,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
decimal minArea = decimal(0.0);
decimal maxArea = decimal(0.0);
bool isPreviousAreaPositive = true;
for (uint i=0; i < candidatePointsIndices.size(); i++) {
for (uint i=0; i < nbCandidatePoints; i++) {
const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@ -1310,12 +1365,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
if (maxArea > (-minArea)) {
isPreviousAreaPositive = true;
pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex];
candidatePointsIndices.removeAt(thirdPointMaxAreaIndex);
removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints);
}
else {
isPreviousAreaPositive = false;
pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex];
candidatePointsIndices.removeAt(thirdPointMinAreaIndex);
removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints);
}
nbReducedPoints = 3;
@ -1328,7 +1383,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
decimal area;
// For each remaining candidate points
for (uint i=0; i < candidatePointsIndices.size(); i++) {
for (uint i=0; i < nbCandidatePoints; i++) {
const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
@ -1365,14 +1420,22 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
}
}
pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
candidatePointsIndices.removeAt(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]);
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(uint array[], uint8 index, uint8& arraySize) const {
assert(index < arraySize);
assert(arraySize > 0);
array[index] = array[arraySize - 1];
arraySize--;
}
// Report contacts and triggers