From 44e07e0bd9f771360f6385a39cbdce4765fba85f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 9 Nov 2019 23:55:54 +0100 Subject: [PATCH] Working on making middle-phase collision detection faster --- CMakeLists.txt | 4 +- src/body/CollisionBody.cpp | 3 + src/collision/ContactManifoldInfo.h | 6 +- src/collision/ContactPair.h | 6 +- src/collision/broadphase/DynamicAABBTree.cpp | 2 + .../CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp | 23 +- .../CapsuleVsCapsuleNarrowPhaseInfoBatch.h | 4 +- .../narrowphase/CollisionDispatch.cpp | 2 + src/collision/narrowphase/CollisionDispatch.h | 8 + .../narrowphase/GJK/GJKAlgorithm.cpp | 2 +- .../narrowphase/NarrowPhaseInfoBatch.cpp | 31 +- .../narrowphase/NarrowPhaseInfoBatch.h | 25 +- .../narrowphase/NarrowPhaseInput.cpp | 23 +- src/collision/narrowphase/NarrowPhaseInput.h | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 2 +- .../SphereVsCapsuleNarrowPhaseInfoBatch.cpp | 23 +- .../SphereVsCapsuleNarrowPhaseInfoBatch.h | 4 +- .../SphereVsSphereNarrowPhaseInfoBatch.cpp | 22 +- .../SphereVsSphereNarrowPhaseInfoBatch.h | 4 +- src/collision/shapes/CollisionShape.h | 1 + src/collision/shapes/ConcaveMeshShape.cpp | 4 + src/collision/shapes/HeightFieldShape.cpp | 2 + src/collision/shapes/SphereShape.cpp | 2 + src/collision/shapes/TriangleShape.cpp | 2 + src/configuration.h | 2 + src/containers/List.h | 24 +- src/engine/Entity.h | 2 +- src/engine/OverlappingPair.cpp | 115 ----- src/engine/OverlappingPairs.cpp | 267 +++++++++++ .../{OverlappingPair.h => OverlappingPairs.h} | 193 ++++---- src/mathematics/mathematics_functions.cpp | 8 + src/mathematics/mathematics_functions.h | 6 + src/systems/CollisionDetectionSystem.cpp | 436 ++++++++++++------ src/systems/CollisionDetectionSystem.h | 37 +- test/tests/containers/TestList.h | 16 + 35 files changed, 882 insertions(+), 433 deletions(-) delete mode 100644 src/engine/OverlappingPair.cpp create mode 100644 src/engine/OverlappingPairs.cpp rename src/engine/{OverlappingPair.h => OverlappingPairs.h} (54%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32e79967..e728d48d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -141,7 +141,7 @@ SET (REACTPHYSICS3D_HEADERS "src/engine/Island.h" "src/engine/Islands.h" "src/engine/Material.h" - "src/engine/OverlappingPair.h" + "src/engine/OverlappingPairs.h" "src/engine/Timer.h" "src/systems/BroadPhaseSystem.h" "src/components/Components.h" @@ -239,7 +239,7 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/DynamicsWorld.cpp" "src/engine/Island.cpp" "src/engine/Material.cpp" - "src/engine/OverlappingPair.cpp" + "src/engine/OverlappingPairs.cpp" "src/engine/Timer.cpp" "src/engine/Entity.cpp" "src/engine/EntityManager.cpp" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 3eaeb3a7..274e5313 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -183,6 +183,9 @@ void CollisionBody::removeCollisionShape(ProxyShape* proxyShape) { // Remove the proxy-shape component mWorld.mProxyShapesComponents.removeComponent(proxyShape->getEntity()); + // Destroy the entity + mWorld.mEntityManager.destroyEntity(proxyShape->getEntity()); + // Call the constructor of the proxy-shape proxyShape->~ProxyShape(); diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index 9d3fed31..c0ec9062 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -29,7 +29,7 @@ // Libraries #include "mathematics/mathematics.h" #include "configuration.h" -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -49,12 +49,12 @@ struct ContactManifoldInfo { List potentialContactPointsIndices; /// Overlapping pair id - OverlappingPair::OverlappingPairId pairId; + uint64 pairId; // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator) + ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator) : potentialContactPointsIndices(allocator), pairId(pairId) { } diff --git a/src/collision/ContactPair.h b/src/collision/ContactPair.h index d8fbf46c..c4e14d43 100644 --- a/src/collision/ContactPair.h +++ b/src/collision/ContactPair.h @@ -29,7 +29,7 @@ // Libraries #include "mathematics/mathematics.h" #include "configuration.h" -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -45,7 +45,7 @@ struct ContactPair { // -------------------- Attributes -------------------- // /// Overlapping pair Id - OverlappingPair::OverlappingPairId pairId; + uint64 pairId; /// Indices of the potential contact manifolds List potentialContactManifoldsIndices; @@ -83,7 +83,7 @@ struct ContactPair { // -------------------- Methods -------------------- // /// Constructor - ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity, + ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity, Entity proxyShape2Entity, uint contactPairIndex, MemoryAllocator& allocator) : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity), proxyShape1Entity(proxyShape1Entity), proxyShape2Entity(proxyShape2Entity), diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index c27262ee..c8fd3584 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -645,6 +645,8 @@ void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const List& no // Report all shapes overlapping with the AABB given in parameter. void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, List& overlappingNodes) const { + RP3D_PROFILE("DynamicAABBTree::reportAllShapesOverlappingWithAABB()", mProfiler); + // Create a stack with the nodes to visit Stack stack(mAllocator, 64); stack.push(mRootNodeID); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp index d90e9e8f..879fb258 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp @@ -30,14 +30,15 @@ using namespace reactphysics3d; // Constructor -CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator) - : NarrowPhaseInfoBatch(allocator), capsule1Radiuses(allocator), capsule2Radiuses(allocator), +CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, + OverlappingPairs& overlappingPairs) + : NarrowPhaseInfoBatch(allocator, overlappingPairs), capsule1Radiuses(allocator), capsule2Radiuses(allocator), capsule1Heights(allocator), capsule2Heights(allocator) { } // Add shapes to be tested during narrow-phase collision detection into the batch -void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, +void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform) { assert(shape1->getType() == CollisionShapeType::CAPSULE); @@ -46,25 +47,29 @@ void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* p const CapsuleShape* capsule1 = static_cast(shape1); const CapsuleShape* capsule2 = static_cast(shape2); + proxyShapeEntities1.add(proxyShape1); + proxyShapeEntities2.add(proxyShape2); capsule1Radiuses.add(capsule1->getRadius()); capsule2Radiuses.add(capsule2->getRadius()); capsule1Heights.add(capsule1->getHeight()); capsule2Heights.add(capsule2->getHeight()); shape1ToWorldTransforms.add(shape1Transform); shape2ToWorldTransforms.add(shape2Transform); - overlappingPairs.add(pair); + overlappingPairIds.add(pairId); contactPoints.add(List(mMemoryAllocator)); isColliding.add(false); // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) - LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId()); lastFrameCollisionInfos.add(lastFrameInfo); } // Initialize the containers using cached capacity void CapsuleVsCapsuleNarrowPhaseInfoBatch::reserveMemory() { - overlappingPairs.reserve(mCachedCapacity); + overlappingPairIds.reserve(mCachedCapacity); + proxyShapeEntities1.reserve(mCachedCapacity); + proxyShapeEntities2.reserve(mCachedCapacity); shape1ToWorldTransforms.reserve(mCachedCapacity); shape2ToWorldTransforms.reserve(mCachedCapacity); lastFrameCollisionInfos.reserve(mCachedCapacity); @@ -85,9 +90,11 @@ void CapsuleVsCapsuleNarrowPhaseInfoBatch::clear() { // allocated in the next frame at a possibly different location in memory (remember that the // location of the allocated memory of a single frame allocator might change between two frames) - mCachedCapacity = overlappingPairs.size(); + mCachedCapacity = overlappingPairIds.size(); - overlappingPairs.clear(true); + overlappingPairIds.clear(true); + proxyShapeEntities1.clear(true); + proxyShapeEntities2.clear(true); shape1ToWorldTransforms.clear(true); shape2ToWorldTransforms.clear(true); lastFrameCollisionInfos.clear(true); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h index 1cdc2c70..645ee76c 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h @@ -55,13 +55,13 @@ struct CapsuleVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { List capsule2Heights; /// Constructor - CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator); + CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); /// Destructor virtual ~CapsuleVsCapsuleNarrowPhaseInfoBatch() = default; /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + virtual void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform); diff --git a/src/collision/narrowphase/CollisionDispatch.cpp b/src/collision/narrowphase/CollisionDispatch.cpp index f1a4c607..bb0a948e 100644 --- a/src/collision/narrowphase/CollisionDispatch.cpp +++ b/src/collision/narrowphase/CollisionDispatch.cpp @@ -200,6 +200,8 @@ void CollisionDispatch::fillInCollisionMatrix() { NarrowPhaseAlgorithmType CollisionDispatch::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, const CollisionShapeType& shape2Type) const { + RP3D_PROFILE("CollisionDispatch::selectNarrowPhaseAlgorithm()", mProfiler); + uint shape1Index = static_cast(shape1Type); uint shape2Index = static_cast(shape2Type); diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 58309c00..d3c223b8 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -106,6 +106,13 @@ class CollisionDispatch { /// use between two types of collision shapes. NarrowPhaseAlgorithmType selectAlgorithm(int type1, int type2); +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public: /// Constructor @@ -201,6 +208,7 @@ inline ConvexPolyhedronVsConvexPolyhedronAlgorithm* CollisionDispatch::getConvex // Set the profiler inline void CollisionDispatch::setProfiler(Profiler* profiler) { + mProfiler = profiler; mSphereVsSphereAlgorithm->setProfiler(profiler); mCapsuleVsCapsuleAlgorithm->setProfiler(profiler); mSphereVsCapsuleAlgorithm->setProfiler(profiler); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 317145b9..11fb5ea2 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include "GJKAlgorithm.h" #include "constraint/ContactPoint.h" -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" #include "collision/shapes/TriangleShape.h" #include "configuration.h" #include "utils/Profiler.h" diff --git a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp index ed989870..7e53dd6a 100644 --- a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp @@ -27,14 +27,15 @@ #include "NarrowPhaseInfoBatch.h" #include "collision/ContactPointInfo.h" #include "collision/shapes/TriangleShape.h" -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" #include using namespace reactphysics3d; // Constructor -NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator) - : mMemoryAllocator(allocator), overlappingPairs(allocator), collisionShapes1(allocator), collisionShapes2(allocator), +NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) + : mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), overlappingPairIds(allocator), + proxyShapeEntities1(allocator), proxyShapeEntities2(allocator), collisionShapes1(allocator), collisionShapes2(allocator), shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator), isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator), lastFrameCollisionInfos(allocator) { @@ -47,11 +48,13 @@ NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() { } // Add shapes to be tested during narrow-phase collision detection into the batch -void NarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, +void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, MemoryAllocator& shapeAllocator) { - overlappingPairs.add(pair); + overlappingPairIds.add(pairId); + proxyShapeEntities1.add(proxyShape1); + proxyShapeEntities2.add(proxyShape2); collisionShapes1.add(shape1); collisionShapes2.add(shape2); shape1ToWorldTransforms.add(shape1Transform); @@ -61,7 +64,7 @@ void NarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionSh isColliding.add(false); // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) - LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId()); lastFrameCollisionInfos.add(lastFrameInfo); } @@ -72,7 +75,7 @@ void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNor assert(penDepth > decimal(0.0)); // Get the memory allocator - MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator(); + MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator(); // Create the contact point info ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) @@ -86,7 +89,7 @@ void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNor void NarrowPhaseInfoBatch::resetContactPoints(uint index) { // Get the memory allocator - MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator(); + MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator(); // For each remaining contact point info for (uint i=0; i < contactPoints[index].size(); i++) { @@ -106,7 +109,9 @@ void NarrowPhaseInfoBatch::resetContactPoints(uint index) { // Initialize the containers using cached capacity void NarrowPhaseInfoBatch::reserveMemory() { - overlappingPairs.reserve(mCachedCapacity); + overlappingPairIds.reserve(mCachedCapacity); + proxyShapeEntities1.reserve(mCachedCapacity); + proxyShapeEntities2.reserve(mCachedCapacity); collisionShapes1.reserve(mCachedCapacity); collisionShapes2.reserve(mCachedCapacity); shape1ToWorldTransforms.reserve(mCachedCapacity); @@ -120,7 +125,7 @@ void NarrowPhaseInfoBatch::reserveMemory() { // Clear all the objects in the batch void NarrowPhaseInfoBatch::clear() { - for (uint i=0; i < overlappingPairs.size(); i++) { + for (uint i=0; i < overlappingPairIds.size(); i++) { assert(contactPoints[i].size() == 0); @@ -141,9 +146,11 @@ void NarrowPhaseInfoBatch::clear() { // allocated in the next frame at a possibly different location in memory (remember that the // location of the allocated memory of a single frame allocator might change between two frames) - mCachedCapacity = overlappingPairs.size(); + mCachedCapacity = overlappingPairIds.size(); - overlappingPairs.clear(true); + overlappingPairIds.clear(true); + proxyShapeEntities1.clear(true); + proxyShapeEntities2.clear(true); collisionShapes1.clear(true); collisionShapes2.clear(true); shape1ToWorldTransforms.clear(true); diff --git a/src/collision/narrowphase/NarrowPhaseInfoBatch.h b/src/collision/narrowphase/NarrowPhaseInfoBatch.h index f42c41e5..9a76642f 100644 --- a/src/collision/narrowphase/NarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_NARROW_PHASE_INFO_BATCH_H // Libraries -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -52,13 +52,22 @@ struct NarrowPhaseInfoBatch { /// Memory allocator MemoryAllocator& mMemoryAllocator; + /// Reference to all the broad-phase overlapping pairs + OverlappingPairs& mOverlappingPairs; + /// Cached capacity uint mCachedCapacity = 0; public: - /// List of Broadphase overlapping pairs - List overlappingPairs; + /// List of Broadphase overlapping pairs ids + List overlappingPairIds; + + /// List of pointers to the first proxy-shapes to test collision with + List proxyShapeEntities1; + + /// List of pointers to the second proxy-shapes to test collision with + List proxyShapeEntities2; /// List of pointers to the first collision shapes to test collision with List collisionShapes1; @@ -85,7 +94,7 @@ struct NarrowPhaseInfoBatch { List lastFrameCollisionInfos; /// Constructor - NarrowPhaseInfoBatch(MemoryAllocator& allocator); + NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); /// Destructor virtual ~NarrowPhaseInfoBatch(); @@ -94,9 +103,9 @@ struct NarrowPhaseInfoBatch { uint getNbObjects() const; /// Add shapes to be tested during narrow-phase collision detection into the batch - void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, MemoryAllocator& shapeAllocator); + void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, MemoryAllocator& shapeAllocator); /// Add a new contact point virtual void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, @@ -114,7 +123,7 @@ struct NarrowPhaseInfoBatch { /// Return the number of objects in the batch inline uint NarrowPhaseInfoBatch::getNbObjects() const { - return overlappingPairs.size(); + return overlappingPairIds.size(); } } diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp index b5004c93..6c00889c 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.cpp +++ b/src/collision/narrowphase/NarrowPhaseInput.cpp @@ -30,36 +30,37 @@ using namespace reactphysics3d; /// Constructor -NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator) - :mSphereVsSphereBatch(allocator), mSphereVsCapsuleBatch(allocator), mCapsuleVsCapsuleBatch(allocator), - mSphereVsConvexPolyhedronBatch(allocator), mCapsuleVsConvexPolyhedronBatch(allocator), - mConvexPolyhedronVsConvexPolyhedronBatch(allocator) { +NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) + :mSphereVsSphereBatch(allocator, overlappingPairs), mSphereVsCapsuleBatch(allocator, overlappingPairs), + mCapsuleVsCapsuleBatch(allocator, overlappingPairs), mSphereVsConvexPolyhedronBatch(allocator, overlappingPairs), + mCapsuleVsConvexPolyhedronBatch(allocator, overlappingPairs), + mConvexPolyhedronVsConvexPolyhedronBatch(allocator, overlappingPairs) { } // Add shapes to be tested during narrow-phase collision detection into the batch -void NarrowPhaseInput::addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, +void NarrowPhaseInput::addNarrowPhaseTest(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, MemoryAllocator& shapeAllocator) { switch (narrowPhaseAlgorithmType) { case NarrowPhaseAlgorithmType::SphereVsSphere: - mSphereVsSphereBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); + mSphereVsSphereBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform); break; case NarrowPhaseAlgorithmType::SphereVsCapsule: - mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); + mSphereVsCapsuleBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform); break; case NarrowPhaseAlgorithmType::CapsuleVsCapsule: - mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform); + mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform); break; case NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron: - mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); break; case NarrowPhaseAlgorithmType::CapsuleVsConvexPolyhedron: - mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); break; case NarrowPhaseAlgorithmType::ConvexPolyhedronVsConvexPolyhedron: - mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); break; case NarrowPhaseAlgorithmType::None: // Must never happen diff --git a/src/collision/narrowphase/NarrowPhaseInput.h b/src/collision/narrowphase/NarrowPhaseInput.h index ee1c3906..f5256155 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.h +++ b/src/collision/narrowphase/NarrowPhaseInput.h @@ -64,10 +64,10 @@ class NarrowPhaseInput { public: /// Constructor - NarrowPhaseInput(MemoryAllocator& allocator); + NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); /// Add shapes to be tested during narrow-phase collision detection into the batch - void addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* shape1, + void addNarrowPhaseTest(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, MemoryAllocator& shapeAllocator); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index ff16e0b7..dbd2a00f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -29,7 +29,7 @@ #include "collision/PolyhedronMesh.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" #include "collision/narrowphase/NarrowPhaseInfoBatch.h" #include "collision/shapes/TriangleShape.h" #include "configuration.h" diff --git a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp index de617979..b5baa27d 100644 --- a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp @@ -31,14 +31,15 @@ using namespace reactphysics3d; // Constructor -SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator) - : NarrowPhaseInfoBatch(allocator), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator), +SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, + OverlappingPairs& overlappingPairs) + : NarrowPhaseInfoBatch(allocator, overlappingPairs), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator), capsuleHeights(allocator) { } // Add shapes to be tested during narrow-phase collision detection into the batch -void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, +void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform) { bool isSphereShape1 = shape1->getType() == CollisionShapeType::SPHERE; @@ -55,19 +56,23 @@ void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pa capsuleHeights.add(capsuleShape->getHeight()); shape1ToWorldTransforms.add(shape1Transform); shape2ToWorldTransforms.add(shape2Transform); - overlappingPairs.add(pair); + overlappingPairIds.add(pairId); + proxyShapeEntities1.add(proxyShape1); + proxyShapeEntities2.add(proxyShape2); contactPoints.add(List(mMemoryAllocator)); isColliding.add(false); // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) - LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId()); lastFrameCollisionInfos.add(lastFrameInfo); } // Initialize the containers using cached capacity void SphereVsCapsuleNarrowPhaseInfoBatch::reserveMemory() { - overlappingPairs.reserve(mCachedCapacity); + overlappingPairIds.reserve(mCachedCapacity); + proxyShapeEntities1.reserve(mCachedCapacity); + proxyShapeEntities2.reserve(mCachedCapacity); shape1ToWorldTransforms.reserve(mCachedCapacity); shape2ToWorldTransforms.reserve(mCachedCapacity); lastFrameCollisionInfos.reserve(mCachedCapacity); @@ -88,9 +93,11 @@ void SphereVsCapsuleNarrowPhaseInfoBatch::clear() { // allocated in the next frame at a possibly different location in memory (remember that the // location of the allocated memory of a single frame allocator might change between two frames) - mCachedCapacity = overlappingPairs.size(); + mCachedCapacity = overlappingPairIds.size(); - overlappingPairs.clear(true); + overlappingPairIds.clear(true); + proxyShapeEntities1.clear(true); + proxyShapeEntities2.clear(true); shape1ToWorldTransforms.clear(true); shape2ToWorldTransforms.clear(true); lastFrameCollisionInfos.clear(true); diff --git a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h index 54923b41..9145967d 100644 --- a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h @@ -55,13 +55,13 @@ struct SphereVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { List capsuleHeights; /// Constructor - SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator); + SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); /// Destructor virtual ~SphereVsCapsuleNarrowPhaseInfoBatch() = default; /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + virtual void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform); diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp index e9ab2b67..27a67f68 100644 --- a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp @@ -30,13 +30,13 @@ using namespace reactphysics3d; // Constructor -SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator) - : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator) { +SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) + : NarrowPhaseInfoBatch(allocator, overlappingPairs), sphere1Radiuses(allocator), sphere2Radiuses(allocator) { } // Add shapes to be tested during narrow-phase collision detection into the batch -void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, +void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform) { assert(shape1->getType() == CollisionShapeType::SPHERE); @@ -47,21 +47,25 @@ void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pai sphere1Radiuses.add(sphere1->getRadius()); sphere2Radiuses.add(sphere2->getRadius()); + proxyShapeEntities1.add(proxyShape1); + proxyShapeEntities2.add(proxyShape2); shape1ToWorldTransforms.add(shape1Transform); shape2ToWorldTransforms.add(shape2Transform); - overlappingPairs.add(pair); + overlappingPairIds.add(pairId); contactPoints.add(List(mMemoryAllocator)); isColliding.add(false); // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) - LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId()); lastFrameCollisionInfos.add(lastFrameInfo); } // Initialize the containers using cached capacity void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() { - overlappingPairs.reserve(mCachedCapacity); + overlappingPairIds.reserve(mCachedCapacity); + proxyShapeEntities1.reserve(mCachedCapacity); + proxyShapeEntities2.reserve(mCachedCapacity); shape1ToWorldTransforms.reserve(mCachedCapacity); shape2ToWorldTransforms.reserve(mCachedCapacity); lastFrameCollisionInfos.reserve(mCachedCapacity); @@ -80,9 +84,11 @@ void SphereVsSphereNarrowPhaseInfoBatch::clear() { // allocated in the next frame at a possibly different location in memory (remember that the // location of the allocated memory of a single frame allocator might change between two frames) - mCachedCapacity = overlappingPairs.size(); + mCachedCapacity = overlappingPairIds.size(); - overlappingPairs.clear(true); + overlappingPairIds.clear(true); + proxyShapeEntities1.clear(true); + proxyShapeEntities2.clear(true); shape1ToWorldTransforms.clear(true); shape2ToWorldTransforms.clear(true); lastFrameCollisionInfos.clear(true); diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h index 953cd01e..4087f90f 100644 --- a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h +++ b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h @@ -49,13 +49,13 @@ struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { List sphere2Radiuses; /// Constructor - SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator); + SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); /// Destructor virtual ~SphereVsSphereNarrowPhaseInfoBatch() override = default; /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + virtual void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform); diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 2f2d5611..edf1f9c6 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -29,6 +29,7 @@ // Libraries #include #include "configuration.h" +#include "utils/Profiler.h" /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index bf5d2eeb..18518bad 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -120,6 +120,8 @@ void ConcaveMeshShape::computeOverlappingTriangles(const AABB& localAABB, List& triangleVerticesNormals, List& shapeIds, MemoryAllocator& allocator) const { + RP3D_PROFILE("ConcaveMeshShape::computeOverlappingTriangles()", mProfiler); + // Compute the nodes of the internal AABB tree that are overlapping with the AABB List overlappingNodes(allocator); mDynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, overlappingNodes); @@ -181,6 +183,8 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh // Compute the shape Id for a given triangle of the mesh uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const { + RP3D_PROFILE("ConcaveMeshShape::computeTriangleShapeId()", mProfiler); + uint shapeId = 0; uint i=0; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 7662bb1e..7b304343 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -95,6 +95,8 @@ void HeightFieldShape::computeOverlappingTriangles(const AABB& localAABB, List& triangleVerticesNormals, List& shapeIds, MemoryAllocator& allocator) const { + RP3D_PROFILE("HeightFieldShape::computeOverlappingTriangles()", mProfiler); + // Compute the non-scaled AABB Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 1dcc1dee..9f1035d2 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -49,6 +49,8 @@ SphereShape::SphereShape(decimal radius) */ void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const { + RP3D_PROFILE("SphereShape::computeAABB()", mProfiler); + // Get the local extents in x,y and z direction Vector3 extents(mMargin, mMargin, mMargin); diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 1364adf1..178c9b16 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -221,6 +221,8 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& */ void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const { + RP3D_PROFILE("TriangleShape::computeAABB()", mProfiler); + const Vector3 worldPoint1 = transform * mPoints[0]; const Vector3 worldPoint2 = transform * mPoints[1]; const Vector3 worldPoint3 = transform * mPoints[2]; diff --git a/src/configuration.h b/src/configuration.h index fa0fe421..c38a01b0 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -60,6 +60,8 @@ using int16 = std::int16_t; using uint16 = std::uint16_t; using int32 = std::int32_t; using uint32 = std::uint32_t; +using int64 = std::int64_t; +using uint64 = std::uint64_t; // TODO : Delete this struct Entity; diff --git a/src/containers/List.h b/src/containers/List.h index 1b1f40aa..8139069c 100755 --- a/src/containers/List.h +++ b/src/containers/List.h @@ -320,8 +320,7 @@ class List { return removeAt(it.mCurrentIndex); } - /// Remove an element from the list at a given index and return an - /// iterator pointing to the element after the removed one (or end() if none) + /// Remove an element from the list at a given index (all the following items will be moved) Iterator removeAt(uint index) { assert(index >= 0 && index < mSize); @@ -343,6 +342,27 @@ class List { return Iterator(mBuffer, index, mSize); } + /// Remove an element from the list at a given index (the deleted item will be replaced by the last one of the list) + void removeAtAndReplaceWithLast(uint index) { + + assert(index >= 0 && index < mSize); + + // Call the destructor on the item to remove + (static_cast(mBuffer)[index]).~T(); + + // If the item to remove is not the last one + if (index < mSize - 1) { + + // Copy the last item of the array to the location of the deleted item + new (static_cast(mBuffer) + index * sizeof(T)) T(static_cast(mBuffer)[mSize - 1]); + + // Call the destructor of the last item of the array + (static_cast(mBuffer)[mSize - 1]).~T(); + } + + mSize--; + } + /// Append another list at the end of the current one void addRange(const List& list) { diff --git a/src/engine/Entity.h b/src/engine/Entity.h index 222195fd..7d82925b 100644 --- a/src/engine/Entity.h +++ b/src/engine/Entity.h @@ -136,7 +136,7 @@ namespace std { size_t operator()(const reactphysics3d::Entity& entity) const { - return entity.id; + return std::hash{}(entity.id); } }; } diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp deleted file mode 100644 index ca86ce40..00000000 --- a/src/engine/OverlappingPair.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2018 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include "OverlappingPair.h" -#include "containers/containers_common.h" -#include "collision/ContactPointInfo.h" - -using namespace reactphysics3d; - -// Constructor -OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, - const WorldSettings& worldSettings) - : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mProxyShape1(shape1->getEntity()), mProxyShape2(shape2->getEntity()), - mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator), - mLastFrameCollisionInfos(mPersistentAllocator), mWorldSettings(worldSettings), mNeedToTestOverlap(false) { - -} - -// Destructor -OverlappingPair::~OverlappingPair() { - - RP3D_PROFILE("OverlappingPair::~OverlappingPair()", mProfiler); - - // Remove all the remaining last frame collision info - for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { - - // Call the constructor - it->second->~LastFrameCollisionInfo(); - - // Release memory - mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - } -} - -// Add a new last frame collision info if it does not exist for the given shapes already -LastFrameCollisionInfo* OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { - - RP3D_PROFILE("OverlappingPair::addLastFrameInfoIfNecessary()", mProfiler); - - // Try to get the corresponding last frame collision info - const ShapeIdPair shapeIdPair(shapeId1, shapeId2); - auto it = mLastFrameCollisionInfos.find(shapeIdPair); - - // If there is no collision info for those two shapes already - if (it == mLastFrameCollisionInfos.end()) { - - // Create a new collision info - LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) - LastFrameCollisionInfo(); - - // Add it into the map of collision infos - mLastFrameCollisionInfos.add(Pair(shapeIdPair, collisionInfo)); - - return collisionInfo; - } - else { - - // The existing collision info is not obsolete - it->second->isObsolete = false; - - return it->second; - } -} - -// Delete all the obsolete last frame collision info -void OverlappingPair::clearObsoleteLastFrameCollisionInfos() { - - RP3D_PROFILE("OverlappingPair::clearObsoleteLastFrameCollisionInfos()", mProfiler); - - // For each collision info - for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) { - - // If the collision info is obsolete - if (it->second->isObsolete) { - - // Delete it - it->second->~LastFrameCollisionInfo(); - mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - - it = mLastFrameCollisionInfos.remove(it); - } - else { // If the collision info is not obsolete - - // Do not delete it but mark it as obsolete - it->second->isObsolete = true; - - ++it; - } - } -} diff --git a/src/engine/OverlappingPairs.cpp b/src/engine/OverlappingPairs.cpp new file mode 100644 index 00000000..007af276 --- /dev/null +++ b/src/engine/OverlappingPairs.cpp @@ -0,0 +1,267 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include +#include "OverlappingPairs.h" +#include "containers/containers_common.h" +#include "collision/ContactPointInfo.h" + +using namespace reactphysics3d; + +// Constructor +OverlappingPairs::OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ProxyShapeComponents& proxyShapeComponents) + : mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator), + mMapPairIdToPairIndex(persistentMemoryAllocator), + mConvexPairIds(mPersistentAllocator), mConvexProxyShapes1(mPersistentAllocator), mConvexProxyShapes2(mPersistentAllocator), + mConcavePairIds(mPersistentAllocator), mConcaveProxyShapes1(mPersistentAllocator), mConcaveProxyShapes2(mPersistentAllocator), + mConvexLastFrameCollisionInfos(mPersistentAllocator), mConcaveLastFrameCollisionInfos(mPersistentAllocator), + mConvexNeedToTestOverlap(mPersistentAllocator), mConcaveNeedToTestOverlap(mPersistentAllocator), + mProxyShapeComponents(proxyShapeComponents) { + +} + +// Destructor +OverlappingPairs::~OverlappingPairs() { + +} + +/// Add an overlapping pair +uint64 OverlappingPairs::addPair(ProxyShape* shape1, ProxyShape* shape2) { + + // TODO : Maybe use entities in parameters + + const CollisionShape* collisionShape1 = mProxyShapeComponents.getCollisionShape(shape1->getEntity()); + const CollisionShape* collisionShape2 = mProxyShapeComponents.getCollisionShape(shape2->getEntity()); + + const uint32 shape1Id = static_cast(shape1->getBroadPhaseId()); + const uint32 shape2Id = static_cast(shape2->getBroadPhaseId()); + + // Compute a unique id for the overlapping pair + const uint64 pairId = pairNumbers(std::max(shape1Id, shape2Id), std::min(shape1Id, shape2Id)); + + // If both shapes are convex + if (collisionShape1->isConvex() && collisionShape2->isConvex()) { + + const uint nbConvexPairs = static_cast(mConvexPairIds.size()); + + mConvexPairIds.add(pairId); + mConvexProxyShapes1.add(shape1->getEntity()); + mConvexProxyShapes2.add(shape2->getEntity()); + mConvexNeedToTestOverlap.add(false); + + // TODO: Make sure we use the correct allocator here + mConvexLastFrameCollisionInfos.add(Map(mPersistentAllocator)); + + // Add a mapping to the index in the internal arrays + mMapPairIdToPairIndex.add(Pair(pairId, PairLocation(true, nbConvexPairs))); + } + else { + + const uint nbConcavePairs = static_cast(mConcavePairIds.size()); + + mConcavePairIds.add(pairId); + mConcaveProxyShapes1.add(shape1->getEntity()); + mConcaveProxyShapes2.add(shape2->getEntity()); + mConcaveNeedToTestOverlap.add(true); + + // TODO: Make sure we use the correct allocator here + mConcaveLastFrameCollisionInfos.add(Map(mPersistentAllocator)); + + // Add a mapping to the index in the internal arrays + mMapPairIdToPairIndex.add(Pair(pairId, PairLocation(false, nbConcavePairs))); + } + + return pairId; +} + +// Remove an overlapping pair +uint64 OverlappingPairs::removePair(uint64 pairId) { + + RP3D_PROFILE("OverlappingPairs::removePair()", mProfiler); + + assert(mMapPairIdToPairIndex.containsKey(pairId)); + + const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; + + if (pairLocation.isConvexVsConvex) { + + assert(pairLocation.pairIndex < mConvexPairIds.size()); + + const uint64 lastPairId = mConvexPairIds[mConvexPairIds.size() - 1]; + const PairLocation lastPairLocation = mMapPairIdToPairIndex[lastPairId]; + + // Remap the last pair location + if (pairLocation.pairIndex < mConvexPairIds.size() - 1) { + mMapPairIdToPairIndex[lastPairId] = PairLocation(lastPairLocation.isConvexVsConvex, pairLocation.pairIndex); + } + + // Remove the pair (the pair is replaced by the last one of the lists) + mConvexPairIds.removeAtAndReplaceWithLast(pairLocation.pairIndex); + mConvexProxyShapes1.removeAtAndReplaceWithLast(pairLocation.pairIndex); + mConvexProxyShapes2.removeAtAndReplaceWithLast(pairLocation.pairIndex); + mConvexNeedToTestOverlap.removeAtAndReplaceWithLast(pairLocation.pairIndex); + } + else { + + assert(pairLocation.pairIndex < mConcavePairIds.size()); + + const uint64 lastPairId = mConcavePairIds[mConcavePairIds.size() - 1]; + const PairLocation lastPairLocation = mMapPairIdToPairIndex[lastPairId]; + + // Remap the last pair location + if (pairLocation.pairIndex < mConcavePairIds.size() - 1) { + mMapPairIdToPairIndex[lastPairId] = PairLocation(lastPairLocation.isConvexVsConvex, pairLocation.pairIndex); + } + + // Remove the pair (the pair is replaced by the last one of the lists) + mConcavePairIds.removeAtAndReplaceWithLast(pairLocation.pairIndex); + mConcaveProxyShapes1.removeAtAndReplaceWithLast(pairLocation.pairIndex); + mConcaveProxyShapes2.removeAtAndReplaceWithLast(pairLocation.pairIndex); + mConcaveNeedToTestOverlap.removeAtAndReplaceWithLast(pairLocation.pairIndex); + } + + mMapPairIdToPairIndex.remove(pairId); + + List>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ? + mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos; + + // Remove all the remaining last frame collision info + for (auto it = lastFrameCollisionInfos[pairLocation.pairIndex].begin(); it != lastFrameCollisionInfos[pairLocation.pairIndex].end(); ++it) { + + // Call the constructor + it->second->~LastFrameCollisionInfo(); + + // Release memory + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + } + + lastFrameCollisionInfos.removeAtAndReplaceWithLast(pairLocation.pairIndex); +} + +// Try to find a pair with a given id, return true if the pair is found and the corresponding PairLocation +bool OverlappingPairs::findPair(uint64 pairId, PairLocation& pairLocation) { + + auto it = mMapPairIdToPairIndex.find(pairId); + if (it != mMapPairIdToPairIndex.end()) { + pairLocation = it->second; + return true; + } + + return false; +} + +// Add a new last frame collision info if it does not exist for the given shapes already +LastFrameCollisionInfo* OverlappingPairs::addLastFrameInfoIfNecessary(uint64 pairId, uint shapeId1, uint shapeId2) { + + RP3D_PROFILE("OverlappingPairs::addLastFrameInfoIfNecessary()", mProfiler); + + assert(mMapPairIdToPairIndex.containsKey(pairId)); + + PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; + List>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ? + mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos; + + // Try to get the corresponding last frame collision info + const ShapeIdPair shapeIdPair(shapeId1, shapeId2); + // TODO : Remove test + auto it = lastFrameCollisionInfos[pairLocation.pairIndex].find(shapeIdPair); + + // If there is no collision info for those two shapes already + if (it == lastFrameCollisionInfos[pairLocation.pairIndex].end()) { + + // Create a new collision info + LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) + LastFrameCollisionInfo(); + + // Add it into the map of collision infos + lastFrameCollisionInfos[pairLocation.pairIndex].add(Pair(shapeIdPair, collisionInfo)); + + return collisionInfo; + } + else { + + // The existing collision info is not obsolete + it->second->isObsolete = false; + + return it->second; + } +} + +// Delete all the obsolete last frame collision info +void OverlappingPairs::clearObsoleteLastFrameCollisionInfos() { + + RP3D_PROFILE("OverlappingPairs::clearObsoleteLastFrameCollisionInfos()", mProfiler); + + // For each convex vs convex overlapping pair + for (uint p=0; p < mConvexLastFrameCollisionInfos.size(); p++) { + + // For each collision info + for (auto it = mConvexLastFrameCollisionInfos[p].begin(); it != mConvexLastFrameCollisionInfos[p].end(); ) { + + // If the collision info is obsolete + if (it->second->isObsolete) { + + // Delete it + it->second->~LastFrameCollisionInfo(); + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + + it = mConvexLastFrameCollisionInfos[p].remove(it); + } + else { // If the collision info is not obsolete + + // Do not delete it but mark it as obsolete + it->second->isObsolete = true; + + ++it; + } + } + } + + // For each convex vs concave overlapping pair + for (uint p=0; p < mConcaveLastFrameCollisionInfos.size(); p++) { + + // For each collision info + for (auto it = mConcaveLastFrameCollisionInfos[p].begin(); it != mConcaveLastFrameCollisionInfos[p].end(); ) { + + // If the collision info is obsolete + if (it->second->isObsolete) { + + // Delete it + it->second->~LastFrameCollisionInfo(); + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + + it = mConcaveLastFrameCollisionInfos[p].remove(it); + } + else { // If the collision info is not obsolete + + // Do not delete it but mark it as obsolete + it->second->isObsolete = true; + + ++it; + } + } + } +} diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPairs.h similarity index 54% rename from src/engine/OverlappingPair.h rename to src/engine/OverlappingPairs.h index bf46290c..23800c63 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPairs.h @@ -32,6 +32,7 @@ #include "containers/Pair.h" #include "containers/containers_common.h" #include "utils/Profiler.h" +#include "components/ProxyShapeComponents.h" #include /// ReactPhysics3D namespace @@ -88,52 +89,97 @@ struct LastFrameCollisionInfo { } }; -// Class OverlappingPair +// Class OverlappingPairs /** - * This class represents a pair of two proxy collision shapes that are overlapping - * during the broad-phase collision detection. It is created when + * This class contains pairs of two proxy collision shapes that are overlapping + * during the broad-phase collision detection. A pair is created when * the two proxy collision shapes start to overlap and is destroyed when they do not - * overlap anymore. This class contains a contact manifold that + * overlap anymore. Each contains a contact manifold that * store all the contact points between the two bodies. */ -class OverlappingPair { +class OverlappingPairs { public: - using OverlappingPairId = Pair; + // TODO : Try to use a pairing function like pairNumbers() here using ShapeIdPair = Pair; private: + /// Structure PairLocation + struct PairLocation { + + /// True if the pair is a convex vs convex overlap + bool isConvexVsConvex; + + /// Index of the overlapping pair in the internal arrays + uint32 pairIndex; + + /// Constructor + PairLocation() :isConvexVsConvex(true), pairIndex(0) { + + } + + /// Constructor + PairLocation(bool isConvexVsConvex, uint32 index) + :isConvexVsConvex(isConvexVsConvex), pairIndex(index) { + + } + }; + // -------------------- Attributes -------------------- // - /// Pair ID - OverlappingPairId mPairID; - - /// Entity of the first proxy-shape of the overlapping pair - Entity mProxyShape1; - - /// Entity of the second proxy-shape of the overlapping pair - Entity mProxyShape2; - /// Persistent memory allocator MemoryAllocator& mPersistentAllocator; /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo MemoryAllocator& mTempMemoryAllocator; + /// Map a pair id to its local location + Map mMapPairIdToPairIndex; + + /// Ids of the convex vs convex pairs + // TODO : Check if we need this array + List mConvexPairIds; + + /// List of Entity of the first proxy-shape of the convex vs convex pairs + List mConvexProxyShapes1; + + /// List of Entity of the second proxy-shape of the convex vs convex pairs + List mConvexProxyShapes2; + + /// Ids of the convex vs concave pairs + // TODO : Check if we need this array + List mConcavePairIds; + + /// List of Entity of the first proxy-shape of the convex vs concave pairs + List mConcaveProxyShapes1; + + /// List of Entity of the second proxy-shape of the convex vs concave pairs + List mConcaveProxyShapes2; + /// Temporal coherence collision data for each overlapping collision shapes of this pair. /// Temporal coherence data store collision information about the last frame. /// If two convex shapes overlap, we have a single collision data but if one shape is concave, /// we might have collision data for several overlapping triangles. The key in the map is the /// shape Ids of the two collision shapes. - Map mLastFrameCollisionInfos; + List> mConvexLastFrameCollisionInfos; - /// World settings - const WorldSettings& mWorldSettings; + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. The key in the map is the + /// shape Ids of the two collision shapes. + List> mConcaveLastFrameCollisionInfos; - /// True if we need to test if the overlapping pair of shapes still overlap - bool mNeedToTestOverlap; + /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap + List mConvexNeedToTestOverlap; + + /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap + List mConcaveNeedToTestOverlap; + + /// Reference to the proxy-shapes components + ProxyShapeComponents& mProxyShapeComponents; #ifdef IS_PROFILING_ACTIVE @@ -147,51 +193,45 @@ class OverlappingPair { // -------------------- Methods -------------------- // /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& persistentMemoryAllocator, - MemoryAllocator& temporaryMemoryAllocator, const WorldSettings& worldSettings); + OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, + ProxyShapeComponents& proxyShapeComponents); /// Destructor - ~OverlappingPair(); + ~OverlappingPairs(); /// Deleted copy-constructor - OverlappingPair(const OverlappingPair& pair) = delete; + OverlappingPairs(const OverlappingPairs& pair) = delete; /// Deleted assignment operator - OverlappingPair& operator=(const OverlappingPair& pair) = delete; + OverlappingPairs& operator=(const OverlappingPairs& pair) = delete; - /// Return the Id of the pair - OverlappingPairId getId() const; + /// Add an overlapping pair + uint64 addPair(ProxyShape* shape1, ProxyShape* shape2); + + /// Remove an overlapping pair + uint64 removePair(uint64 pairId); + + /// Try to find a pair with a given id, return true if the pair is found and the corresponding PairLocation + bool findPair(uint64 pairId, PairLocation& pairLocation); /// Return the entity of the first proxy-shape - Entity getProxyShape1() const; + Entity getProxyShape1(uint64 pairId) const; /// Return the entity of the second proxy-shape - Entity getProxyShape2() const; + Entity getProxyShape2(uint64 pairId) const; /// Return the last frame collision info - LastFrameCollisionInfo* getLastFrameCollisionInfo(ShapeIdPair& shapeIds); + LastFrameCollisionInfo* getLastFrameCollisionInfo(uint64, ShapeIdPair& shapeIds); /// Return a reference to the temporary memory allocator MemoryAllocator& getTemporaryAllocator(); - /// Return true if we need to test if the overlapping pair of shapes still overlap - bool needToTestOverlap() const; - - /// Set to true if we need to test if the overlapping pair of shapes still overlap - void setNeedToTestOverlap(bool needToTestOverlap); - /// Add a new last frame collision info if it does not exist for the given shapes already - LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2); - - /// Return the last frame collision info for a given pair of shape ids - LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const; + LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint64 pairId, uint shapeId1, uint shapeId2); /// Delete all the obsolete last frame collision info void clearObsoleteLastFrameCollisionInfos(); - /// Return the pair of bodies index - static OverlappingPairId computeID(int shape1BroadPhaseId, int shape2BroadPhaseId); - /// Return the pair of bodies index of the pair static bodypair computeBodiesIndexPair(Entity body1Entity, Entity body2Entity); @@ -205,27 +245,35 @@ class OverlappingPair { // -------------------- Friendship -------------------- // friend class DynamicsWorld; + friend class CollisionDetectionSystem; }; -// Return the Id of the pair -inline OverlappingPair::OverlappingPairId OverlappingPair::getId() const { - return mPairID; -} - // Return the entity of the first proxy-shape -inline Entity OverlappingPair::getProxyShape1() const { - return mProxyShape1; +inline Entity OverlappingPairs::getProxyShape1(uint64 pairId) const { + assert(mMapPairIdToPairIndex.containsKey(pairId)); + const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; + const List proxyShapes1 = pairLocation.isConvexVsConvex ? mConvexProxyShapes1 : mConcaveProxyShapes1; + return proxyShapes1[pairLocation.pairIndex]; } // Return the entity of the second proxy-shape -inline Entity OverlappingPair::getProxyShape2() const { - return mProxyShape2; -} +inline Entity OverlappingPairs::getProxyShape2(uint64 pairId) const { + assert(mMapPairIdToPairIndex.containsKey(pairId)); + const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; + const List proxyShapes2 = pairLocation.isConvexVsConvex ? mConvexProxyShapes2 : mConcaveProxyShapes2; + return proxyShapes2[pairLocation.pairIndex]; +} // Return the last frame collision info for a given shape id or nullptr if none is found -inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeIdPair& shapeIds) { - Map::Iterator it = mLastFrameCollisionInfos.find(shapeIds); - if (it != mLastFrameCollisionInfos.end()) { +inline LastFrameCollisionInfo* OverlappingPairs::getLastFrameCollisionInfo(uint64 pairId, ShapeIdPair& shapeIds) { + + assert(mMapPairIdToPairIndex.containsKey(pairId)); + const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; + const List>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ? + mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos; + + Map::Iterator it = lastFrameCollisionInfos[pairLocation.pairIndex].find(shapeIds); + if (it != lastFrameCollisionInfos[pairLocation.pairIndex].end()) { return it->second; } @@ -233,19 +281,7 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeI } // Return the pair of bodies index -inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1BroadPhaseId, int shape2BroadPhaseId) { - assert(shape1BroadPhaseId >= 0 && shape2BroadPhaseId >= 0); - - // Construct the pair of body index - OverlappingPairId pairID = shape1BroadPhaseId < shape2BroadPhaseId ? - OverlappingPairId(shape1BroadPhaseId, shape2BroadPhaseId) : - OverlappingPairId(shape2BroadPhaseId, shape1BroadPhaseId); - assert(pairID.first != pairID.second); - return pairID; -} - -// Return the pair of bodies index -inline bodypair OverlappingPair::computeBodiesIndexPair(Entity body1Entity, Entity body2Entity) { +inline bodypair OverlappingPairs::computeBodiesIndexPair(Entity body1Entity, Entity body2Entity) { // Construct the pair of body index bodypair indexPair = body1Entity.id < body2Entity.id ? @@ -256,29 +292,14 @@ inline bodypair OverlappingPair::computeBodiesIndexPair(Entity body1Entity, Enti } // Return a reference to the temporary memory allocator -inline MemoryAllocator& OverlappingPair::getTemporaryAllocator() { +inline MemoryAllocator& OverlappingPairs::getTemporaryAllocator() { return mTempMemoryAllocator; } -// Return true if we need to test if the overlapping pair of shapes still overlap -inline bool OverlappingPair::needToTestOverlap() const { - return mNeedToTestOverlap; -} - -// Set to true if we need to test if the overlapping pair of shapes still overlap -inline void OverlappingPair::setNeedToTestOverlap(bool needToTestOverlap) { - mNeedToTestOverlap = needToTestOverlap; -} - -// Return the last frame collision info for a given pair of shape ids -inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const { - return mLastFrameCollisionInfos[ShapeIdPair(shapeId1, shapeId2)]; -} - #ifdef IS_PROFILING_ACTIVE // Set the profiler -inline void OverlappingPair::setProfiler(Profiler* profiler) { +inline void OverlappingPairs::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 9c87818e..98689953 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -404,4 +404,12 @@ bool reactphysics3d::isPrimeNumber(int number) { return number == 2; } +// Return an unique integer from two integer numbers (pairing function) +/// Here we assume that the two parameter numbers are sorted such that +/// number1 = max(number1, number2) +/// http://szudzik.com/ElegantPairing.pdf +uint64 reactphysics3d::pairNumbers(uint32 number1, uint32 number2) { + assert(number1 == std::max(number1, number2)); + return number1 * number1 + number1 + number2; +} diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index f81aba84..a60b47b8 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -129,6 +129,12 @@ decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNo /// Return true if the given number is prime bool isPrimeNumber(int number); +/// Return an unique integer from two integer numbers (pairing function) +/// Here we assume that the two parameter numbers are sorted such that +/// number1 = max(number1, number2) +/// http://szudzik.com/ElegantPairing.pdf +uint64 pairNumbers(uint32 number1, uint32 number2); + } diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index a2f2447b..e98f7978 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -55,10 +55,10 @@ CollisionDetectionSystem::CollisionDetectionSystem(CollisionWorld* world, ProxyS RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager) : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), - mOverlappingPairs(mMemoryManager.getPoolAllocator()), + mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mProxyShapesComponents), mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, rigidBodyComponents), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()), - mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), + mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds, mMapPairIdToOverlappingPairContacts mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()), mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2), @@ -74,6 +74,7 @@ CollisionDetectionSystem::CollisionDetectionSystem(CollisionWorld* world, ProxyS mProfiler = nullptr; mCollisionDispatch.setProfiler(mProfiler); + mOverlappingPairs.setProfiler(mProfiler); #endif @@ -119,9 +120,14 @@ void CollisionDetectionSystem::resetNeedToTestOverlap() { RP3D_PROFILE("CollisionDetectionSystem::resetNeedToTestOverlap()", mProfiler); - // For each possible collision pair of bodies - for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - it->second->setNeedToTestOverlap(true); + // For each possible convex vs convex collision pair of bodies + for (uint i=0; i < mOverlappingPairs.mConvexNeedToTestOverlap.size(); i++) { + mOverlappingPairs.mConvexNeedToTestOverlap[i] = true; + } + + // For each possible convex vs concave collision pair of bodies + for (uint i=0; i < mOverlappingPairs.mConcaveNeedToTestOverlap.size(); i++) { + mOverlappingPairs.mConcaveNeedToTestOverlap[i] = true; } } @@ -130,25 +136,29 @@ void CollisionDetectionSystem::removeNonOverlappingPairs() { RP3D_PROFILE("CollisionDetectionSystem::removeNonOverlappingPairs()", mProfiler); - // For each possible collision pair of bodies - for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - - OverlappingPair* pair = it->second; + // For each possible convex vs convex pair of bodies + for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { // Check if we need to test overlap. If so, test if the two shapes are still overlapping. // Otherwise, we destroy the overlapping pair - if (pair->needToTestOverlap() && - !mBroadPhaseSystem.testOverlappingShapes(pair->getProxyShape1(), pair->getProxyShape2())) { + if (mOverlappingPairs.mConvexNeedToTestOverlap[i] && + !mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mConvexProxyShapes1[i], mOverlappingPairs.mConvexProxyShapes2[i])) { - // Destroy the overlapping pair - pair->~OverlappingPair(); - - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); - it = mOverlappingPairs.remove(it); - continue; + mOverlappingPairs.removePair(mOverlappingPairs.mConvexPairIds[i]); + i--; } - else { - ++it; + } + + // For each possible convex vs concave pair of bodies + for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { + + // Check if we need to test overlap. If so, test if the two shapes are still overlapping. + // Otherwise, we destroy the overlapping pair + if (mOverlappingPairs.mConcaveNeedToTestOverlap[i] && + !mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mConcaveProxyShapes1[i], mOverlappingPairs.mConcaveProxyShapes2[i])) { + + mOverlappingPairs.removePair(mOverlappingPairs.mConcavePairIds[i]); + i--; } } } @@ -181,11 +191,11 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List> if (body1Entity != body2Entity) { // Compute the overlapping pair ID - Pair pairID = OverlappingPair::computeID(nodePair.first, nodePair.second); + const uint64 pairId = pairNumbers(std::max(nodePair.first, nodePair.second), std::min(nodePair.first, nodePair.second)); // Check if the overlapping pair already exists - auto itPair = mOverlappingPairs.find(pairID); - if (itPair == mOverlappingPairs.end()) { + OverlappingPairs::PairLocation pairLocation; + if (!mOverlappingPairs.findPair(pairId, pairLocation)) { unsigned short shape1CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity); unsigned short shape2CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity); @@ -200,26 +210,21 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List> ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(proxyShape1Entity); ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(proxyShape2Entity); - // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(), - mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig); - - assert(newPair != nullptr); - - // Add the new overlapping pair - mOverlappingPairs.add(Pair, OverlappingPair*>(pairID, newPair)); - -#ifdef IS_PROFILING_ACTIVE - newPair->setProfiler(mProfiler); -#endif + // Check that at least one collision shape is convex + if (shape1->getCollisionShape()->isConvex() || shape2->getCollisionShape()->isConvex()) { + // Add the new overlapping pair + mOverlappingPairs.addPair(shape1, shape2); + } } } else { // We do not need to test the pair for overlap because it has just been reported that they still overlap - itPair->second->setNeedToTestOverlap(false); + List& pairsNeedToTestOverlap = pairLocation.isConvexVsConvex ? mOverlappingPairs.mConvexNeedToTestOverlap : + mOverlappingPairs.mConcaveNeedToTestOverlap; + + pairsNeedToTestOverlap[pairLocation.pairIndex] = false; } } } @@ -227,23 +232,21 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List> } // Compute the middle-phase collision detection -void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) { +void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) { RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler); // Reserve memory for the narrow-phase input using cached capacity from previous frame narrowPhaseInput.reserveMemory(); - // For each possible collision pair of bodies - for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { + // Remove the obsolete last frame collision infos and mark all the others as obsolete + overlappingPairs.clearObsoleteLastFrameCollisionInfos(); - OverlappingPair* pair = it->second; + // For each possible convex vs convex pair of bodies + for (uint i=0; i < overlappingPairs.mConvexPairIds.size(); i++) { - // Remove the obsolete last frame collision infos and mark all the others as obsolete - pair->clearObsoleteLastFrameCollisionInfos(); - - const Entity proxyShape1Entity = pair->getProxyShape1(); - const Entity proxyShape2Entity = pair->getProxyShape2(); + const Entity proxyShape1Entity = overlappingPairs.mConvexProxyShapes1[i]; + const Entity proxyShape2Entity = overlappingPairs.mConvexProxyShapes2[i]; assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); @@ -258,64 +261,185 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappin mWorld->mRigidBodyComponents.getBodyType(body2Entity) == BodyType::STATIC; // Check that at least one body is enabled (active and awake) and not static + // TODO : Do not test this every frame bool isBody1Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1; bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2; if (!isBody1Active && !isBody2Active) continue; // Check if the bodies are in the set of bodies that cannot collide between each other - bodypair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1Entity, body2Entity); + // TODO : Do not check this every frame but remove and do not create overlapping pairs of bodies in this situation + bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity); CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity); - const bool isShape1Convex = collisionShape1->isConvex(); - const bool isShape2Convex = collisionShape2->isConvex(); + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), + collisionShape2->getType()); - // If both shapes are convex - if (isShape1Convex && isShape2Convex) { + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + narrowPhaseInput.addNarrowPhaseTest(overlappingPairs.mConvexPairIds[i], proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2, + mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), + mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), + algorithmType, mMemoryManager.getSingleFrameAllocator()); - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), - collisionShape2->getType()); + } - // No middle-phase is necessary, simply create a narrow phase info - // for the narrow-phase collision detection - narrowPhaseInput.addNarrowPhaseTest(pair, collisionShape1, collisionShape2, - mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), - mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), - algorithmType, mMemoryManager.getSingleFrameAllocator()); + // For each possible convex vs concave pair of bodies + for (uint i=0; i < overlappingPairs.mConcavePairIds.size(); i++) { - } - // Concave vs Convex algorithm - else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { + const Entity proxyShape1Entity = overlappingPairs.mConcaveProxyShapes1[i]; + const Entity proxyShape2Entity = overlappingPairs.mConcaveProxyShapes2[i]; - computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); - } - // Concave vs Concave shape - else { - // Not handled - continue; - } + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity)); + + const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity); + const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity); + + const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) && + mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC; + const bool isStaticRigidBody2 = mWorld->mRigidBodyComponents.hasComponent(body2Entity) && + mWorld->mRigidBodyComponents.getBodyType(body2Entity) == BodyType::STATIC; + + // Check that at least one body is enabled (active and awake) and not static + // TODO : Do not test this every frame + bool isBody1Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1; + bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2; + if (!isBody1Active && !isBody2Active) continue; + + // Check if the bodies are in the set of bodies that cannot collide between each other + // TODO : Do not check this every frame but remove and do not create overlapping pairs of bodies in this situation + bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); + if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; + + computeConvexVsConcaveMiddlePhase(overlappingPairs.mConcavePairIds[i], proxyShape1Entity, proxyShape2Entity, + mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); } } +// Compute the middle-phase collision detection +void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& convexPairs, List& concavePairs, NarrowPhaseInput& narrowPhaseInput) { + + RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler); + + // Reserve memory for the narrow-phase input using cached capacity from previous frame + narrowPhaseInput.reserveMemory(); + + // Remove the obsolete last frame collision infos and mark all the others as obsolete + mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); + + // For each possible convex vs convex pair of bodies + for (uint p=0; p < convexPairs.size(); p++) { + + const uint64 pairId = convexPairs[p]; + + OverlappingPairs::PairLocation& pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; + assert(pairLoc.isConvexVsConvex); + const uint pairIndex = pairLoc.pairIndex; + + const Entity proxyShape1Entity = mOverlappingPairs.mConvexProxyShapes1[pairIndex]; + const Entity proxyShape2Entity = mOverlappingPairs.mConvexProxyShapes2[pairIndex]; + + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity)); + + const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity); + const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity); + + const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) && + mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC; + const bool isStaticRigidBody2 = mWorld->mRigidBodyComponents.hasComponent(body2Entity) && + mWorld->mRigidBodyComponents.getBodyType(body2Entity) == BodyType::STATIC; + + // Check that at least one body is enabled (active and awake) and not static + // TODO : Do not test this every frame + bool isBody1Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1; + bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2; + if (!isBody1Active && !isBody2Active) continue; + + // Check if the bodies are in the set of bodies that cannot collide between each other + // TODO : Do not check this every frame but remove and do not create overlapping pairs of bodies in this situation + bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); + if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; + + CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity); + CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity); + + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), + collisionShape2->getType()); + + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + narrowPhaseInput.addNarrowPhaseTest(pairId, proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2, + mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), + mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), + algorithmType, mMemoryManager.getSingleFrameAllocator()); + + } + + // For each possible convex vs concave pair of bodies + for (uint p=0; p < concavePairs.size(); p++) { + + const uint64 pairId = concavePairs[p]; + + OverlappingPairs::PairLocation& pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; + assert(!pairLoc.isConvexVsConvex); + const uint pairIndex = pairLoc.pairIndex; + + const Entity proxyShape1Entity = mOverlappingPairs.mConcaveProxyShapes1[pairIndex]; + const Entity proxyShape2Entity = mOverlappingPairs.mConcaveProxyShapes2[pairIndex]; + + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity)); + + const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity); + const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity); + + const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) && + mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC; + const bool isStaticRigidBody2 = mWorld->mRigidBodyComponents.hasComponent(body2Entity) && + mWorld->mRigidBodyComponents.getBodyType(body2Entity) == BodyType::STATIC; + + // Check that at least one body is enabled (active and awake) and not static + // TODO : Do not test this every frame + bool isBody1Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1; + bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2; + if (!isBody1Active && !isBody2Active) continue; + + // Check if the bodies are in the set of bodies that cannot collide between each other + // TODO : Do not check this every frame but remove and do not create overlapping pairs of bodies in this situation + bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); + if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; + + computeConvexVsConcaveMiddlePhase(pairId, proxyShape1Entity, proxyShape2Entity, + mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); + } +} // Compute the concave vs convex middle-phase algorithm for a given pair of bodies -void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, - NarrowPhaseInput& narrowPhaseInput) { +void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairId, Entity proxyShape1, Entity proxyShape2, + MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput) { RP3D_PROFILE("CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase()", mProfiler); - ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(pair->getProxyShape1()); - ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(pair->getProxyShape2()); + ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(proxyShape1); + ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(proxyShape2); ProxyShape* convexProxyShape; ProxyShape* concaveProxyShape; ConvexShape* convexShape; ConcaveShape* concaveShape; + const bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + // Collision shape 1 is convex, collision shape 2 is concave - if (shape1->getCollisionShape()->isConvex()) { + if (isShape1Convex) { convexProxyShape = shape1; convexShape = static_cast(shape1->getCollisionShape()); concaveProxyShape = shape2; @@ -366,15 +490,11 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair #endif - bool isShape1Convex = mProxyShapesComponents.getCollisionShape(pair->getProxyShape1())->isConvex(); - ProxyShape* shape1 = isShape1Convex ? convexProxyShape : concaveProxyShape; - ProxyShape* shape2 = isShape1Convex ? concaveProxyShape : convexProxyShape; - // Create a narrow phase info for the narrow-phase collision detection - narrowPhaseInput.addNarrowPhaseTest(pair, isShape1Convex ? convexShape : triangleShape, + narrowPhaseInput.addNarrowPhaseTest(pairId, proxyShape1, proxyShape2, isShape1Convex ? convexShape : triangleShape, isShape1Convex ? triangleShape : convexShape, - mProxyShapesComponents.getLocalToWorldTransform(shape1->getEntity()), - mProxyShapesComponents.getLocalToWorldTransform(shape2->getEntity()), + mProxyShapesComponents.getLocalToWorldTransform(proxyShape1), + mProxyShapesComponents.getLocalToWorldTransform(proxyShape2), algorithmType, allocator); } } @@ -427,7 +547,7 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow // Process the potential contacts after narrow-phase collision detection void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, + Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, List* contactPairs, Map>& mapBodyToContactPairs) { @@ -543,8 +663,8 @@ void CollisionDetectionSystem::computeSnapshotContactPairs(NarrowPhaseInfoBatch& if (narrowPhaseInfoBatch.isColliding[i]) { // Add the pair of bodies in contact into the list - overlapPairs.add(Pair(mProxyShapesComponents.getBody(narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape1()), - mProxyShapesComponents.getBody(narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape2()))); + overlapPairs.add(Pair(mProxyShapesComponents.getBody(narrowPhaseInfoBatch.proxyShapeEntities1[i]), + mProxyShapesComponents.getBody(narrowPhaseInfoBatch.proxyShapeEntities2[i]))); } narrowPhaseInfoBatch.resetContactPoints(i); @@ -567,7 +687,7 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn List potentialContactPoints(allocator); List potentialContactManifolds(allocator); - Map mapPairIdToContactPairIndex(allocator); + Map mapPairIdToContactPairIndex(allocator); List contactPairs(allocator); List contactManifolds(allocator); List contactPoints(allocator); @@ -838,30 +958,40 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() { // Remove a body from the collision detection void CollisionDetectionSystem::removeProxyCollisionShape(ProxyShape* proxyShape) { - assert(proxyShape->getBroadPhaseId() != -1); - assert(mMapBroadPhaseIdToProxyShapeEntity.containsKey(proxyShape->getBroadPhaseId())); + const int proxyShapeBroadPhaseId = proxyShape->getBroadPhaseId(); - // Remove all the overlapping pairs involving this proxy shape - for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + assert(proxyShapeBroadPhaseId != -1); + assert(mMapBroadPhaseIdToProxyShapeEntity.containsKey(proxyShapeBroadPhaseId)); - OverlappingPair* pair = it->second; + // Remove all the convex vs convex overlapping pairs involving this proxy shape + for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { - if (mProxyShapesComponents.getBroadPhaseId(pair->getProxyShape1()) == proxyShape->getBroadPhaseId() || - mProxyShapesComponents.getBroadPhaseId(pair->getProxyShape2()) == proxyShape->getBroadPhaseId()) { + if (mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConvexProxyShapes1[i]) == proxyShapeBroadPhaseId || + mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConvexProxyShapes2[i]) == proxyShapeBroadPhaseId) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair - pair->~OverlappingPair(); - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); - it = mOverlappingPairs.remove(it); - } - else { - ++it; + // Remove the overlapping pair + mOverlappingPairs.removePair(mOverlappingPairs.mConvexPairIds[i]); + i--; } } - mMapBroadPhaseIdToProxyShapeEntity.remove(proxyShape->getBroadPhaseId()); + // Remove all the convex vs concave overlapping pairs involving this proxy shape + for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { + + if (mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConcaveProxyShapes1[i]) == proxyShapeBroadPhaseId || + mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConcaveProxyShapes2[i]) == proxyShapeBroadPhaseId) { + + // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved + + // Remove the overlapping pair + mOverlappingPairs.removePair(mOverlappingPairs.mConcavePairIds[i]); + i--; + } + } + + mMapBroadPhaseIdToProxyShapeEntity.remove(proxyShapeBroadPhaseId); // Remove the body from the broad-phase mBroadPhaseSystem.removeProxyCollisionShape(proxyShape); @@ -884,7 +1014,7 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, // Convert the potential contact into actual contacts void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, + Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, List* contactPairs, Map>& mapBodyToContactPairs) { @@ -918,7 +1048,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na bool similarManifoldFound = false; // If there is already a contact pair for this overlapping pair - OverlappingPair::OverlappingPairId pairId = narrowPhaseInfoBatch.overlappingPairs[i]->getId(); + const uint64 pairId = narrowPhaseInfoBatch.overlappingPairIds[i]; auto it = mapPairIdToContactPairIndex->find(pairId); ContactPair* pairContact = nullptr; if (it != mapPairIdToContactPairIndex->end()) { @@ -968,8 +1098,8 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na // If the overlapping pair contact does not exists yet if (pairContact == nullptr) { - const Entity proxyShape1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape1(); - const Entity proxyShape2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape2(); + const Entity proxyShape1Entity = narrowPhaseInfoBatch.proxyShapeEntities1[i]; + const Entity proxyShape2Entity = narrowPhaseInfoBatch.proxyShapeEntities2[i]; const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity); const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity); @@ -983,7 +1113,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na newContactPairIndex, mMemoryManager.getPoolAllocator()); contactPairs->add(overlappingPairContact); pairContact = &((*contactPairs)[newContactPairIndex]); - mapPairIdToContactPairIndex->add(Pair(pairId, newContactPairIndex)); + mapPairIdToContactPairIndex->add(Pair(pairId, newContactPairIndex)); auto itbodyContactPairs = mapBodyToContactPairs.find(body1Entity); if (itbodyContactPairs != mapBodyToContactPairs.end()) { @@ -1075,7 +1205,11 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List // If there are two many contact points in the manifold if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) { - Transform shape1LocalToWorldTransoform = mProxyShapesComponents.getLocalToWorldTransform(mOverlappingPairs[manifold.pairId]->getProxyShape1()); + OverlappingPairs::PairLocation pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]; + Entity proxyShape1 = pairLoc.isConvexVsConvex ? mOverlappingPairs.mConvexProxyShapes1[pairLoc.pairIndex] : + mOverlappingPairs.mConcaveProxyShapes1[pairLoc.pairIndex]; + + Transform shape1LocalToWorldTransoform = mProxyShapesComponents.getLocalToWorldTransform(proxyShape1); // Reduce the number of contact points in the manifold reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints); @@ -1313,19 +1447,20 @@ void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, List< // Return true if two bodies overlap (collide) bool CollisionDetectionSystem::testOverlap(CollisionBody* body1, CollisionBody* body2) { - NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs); // Compute the broad-phase collision detection computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); - filterOverlappingPairs(body1->getEntity(), body2->getEntity(), overlappingPairs); + List convexPairs(mMemoryManager.getPoolAllocator()); + List concavePairs(mMemoryManager.getPoolAllocator()); + filterOverlappingPairs(body1->getEntity(), body2->getEntity(), convexPairs, concavePairs); - if (overlappingPairs.size() > 0) { + if (convexPairs.size() > 0 || concavePairs.size() > 0) { // Compute the middle-phase collision detection - computeMiddlePhase(overlappingPairs, narrowPhaseInput); + computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput); // Compute the narrow-phase collision detection return computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, nullptr); @@ -1337,7 +1472,7 @@ bool CollisionDetectionSystem::testOverlap(CollisionBody* body1, CollisionBody* // Report all the bodies that overlap (collide) in the world void CollisionDetectionSystem::testOverlap(OverlapCallback& callback) { - NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs); // Compute the broad-phase collision detection computeBroadPhase(); @@ -1352,19 +1487,20 @@ void CollisionDetectionSystem::testOverlap(OverlapCallback& callback) { // Report all the bodies that overlap (collide) with the body in parameter void CollisionDetectionSystem::testOverlap(CollisionBody* body, OverlapCallback& callback) { - NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs); // Compute the broad-phase collision detection computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); - filterOverlappingPairs(body->getEntity(), overlappingPairs); + List convexPairs(mMemoryManager.getPoolAllocator()); + List concavePairs(mMemoryManager.getPoolAllocator()); + filterOverlappingPairs(body->getEntity(), convexPairs, concavePairs); - if (overlappingPairs.size() > 0) { + if (convexPairs.size() > 0 || concavePairs.size() > 0) { // Compute the middle-phase collision detection - computeMiddlePhase(overlappingPairs, narrowPhaseInput); + computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput); // Compute the narrow-phase collision detection computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, &callback); @@ -1374,19 +1510,20 @@ void CollisionDetectionSystem::testOverlap(CollisionBody* body, OverlapCallback& // Test collision and report contacts between two bodies. void CollisionDetectionSystem::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) { - NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs); // Compute the broad-phase collision detection computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); - filterOverlappingPairs(body1->getEntity(), body2->getEntity(), overlappingPairs); + List convexPairs(mMemoryManager.getPoolAllocator()); + List concavePairs(mMemoryManager.getPoolAllocator()); + filterOverlappingPairs(body1->getEntity(), body2->getEntity(), convexPairs, concavePairs); - if (overlappingPairs.size() > 0) { + if (convexPairs.size() > 0 || concavePairs.size() > 0) { // Compute the middle-phase collision detection - computeMiddlePhase(overlappingPairs, narrowPhaseInput); + computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput); // Compute the narrow-phase collision detection and report contacts computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback); @@ -1396,19 +1533,20 @@ void CollisionDetectionSystem::testCollision(CollisionBody* body1, CollisionBody // Test collision and report all the contacts involving the body in parameter void CollisionDetectionSystem::testCollision(CollisionBody* body, CollisionCallback& callback) { - NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs); // Compute the broad-phase collision detection computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); - filterOverlappingPairs(body->getEntity(), overlappingPairs); + List convexPairs(mMemoryManager.getPoolAllocator()); + List concavePairs(mMemoryManager.getPoolAllocator()); + filterOverlappingPairs(body->getEntity(), convexPairs, concavePairs); - if (overlappingPairs.size() > 0) { + if (convexPairs.size() > 0 || concavePairs.size() > 0) { // Compute the middle-phase collision detection - computeMiddlePhase(overlappingPairs, narrowPhaseInput); + computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput); // Compute the narrow-phase collision detection and report contacts computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback); @@ -1418,7 +1556,7 @@ void CollisionDetectionSystem::testCollision(CollisionBody* body, CollisionCallb // Test collision and report contacts between each colliding bodies in the world void CollisionDetectionSystem::testCollision(CollisionCallback& callback) { - NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs); // Compute the broad-phase collision detection computeBroadPhase(); @@ -1431,33 +1569,51 @@ void CollisionDetectionSystem::testCollision(CollisionCallback& callback) { } // Filter the overlapping pairs to keep only the pairs where a given body is involved -void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const { +void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, List& convexPairs, List& concavePairs) const { // For each possible collision pair of bodies - for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { - OverlappingPair* pair = it->second; + if (mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == bodyEntity || + mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == bodyEntity) { - if (mProxyShapesComponents.getBody(pair->getProxyShape1()) == bodyEntity || - mProxyShapesComponents.getBody(pair->getProxyShape2()) == bodyEntity) { + convexPairs.add(mOverlappingPairs.mConvexPairIds[i]); + } + } - outFilteredPairs.add(Pair, OverlappingPair*>(it->first, pair)); + // For each possible collision pair of bodies + for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { + + if (mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes1[i]) == bodyEntity || + mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes2[i]) == bodyEntity) { + + concavePairs.add(mOverlappingPairs.mConcavePairIds[i]); } } } // Filter the overlapping pairs to keep only the pairs where two given bodies are involved -void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const { +void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List& convexPairs, List& concavePairs) const { + + // TODO : Do not go through all the overlapping pairs here but get all the involded overlapping pairs directly from the bodies // For each possible collision pair of bodies - for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { - OverlappingPair* pair = it->second; + if ((mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == body1Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == body2Entity) || + (mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == body2Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == body1Entity)) { - if ((mProxyShapesComponents.getBody(pair->getProxyShape1()) == body1Entity && mProxyShapesComponents.getBody(pair->getProxyShape2()) == body2Entity) || - (mProxyShapesComponents.getBody(pair->getProxyShape1()) == body2Entity && mProxyShapesComponents.getBody(pair->getProxyShape2()) == body1Entity)) { + convexPairs.add(mOverlappingPairs.mConvexPairIds[i]); + } + } - outFilteredPairs.add(Pair, OverlappingPair*>(it->first, pair)); + // For each possible collision pair of bodies + for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { + + if ((mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes1[i]) == body1Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes2[i]) == body2Entity) || + (mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes1[i]) == body2Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes2[i]) == body1Entity)) { + + concavePairs.add(mOverlappingPairs.mConcavePairIds[i]); } } } diff --git a/src/systems/CollisionDetectionSystem.h b/src/systems/CollisionDetectionSystem.h index f5264245..5ac964d9 100644 --- a/src/systems/CollisionDetectionSystem.h +++ b/src/systems/CollisionDetectionSystem.h @@ -35,7 +35,8 @@ #include "collision/ContactManifoldInfo.h" #include "collision/ContactManifold.h" #include "collision/ContactPair.h" -#include "engine/OverlappingPair.h" +#include "engine/OverlappingPairs.h" +#include "engine/OverlappingPairs.h" #include "collision/narrowphase/NarrowPhaseInput.h" #include "collision/narrowphase/CollisionDispatch.h" #include "containers/Map.h" @@ -89,7 +90,7 @@ class CollisionDetectionSystem { CollisionWorld* mWorld; /// Broad-phase overlapping pairs - OverlappingPairMap mOverlappingPairs; + OverlappingPairs mOverlappingPairs; /// Broad-phase system BroadPhaseSystem mBroadPhaseSystem; @@ -122,18 +123,18 @@ class CollisionDetectionSystem { List* mCurrentContactPairs; /// First map of overlapping pair id to the index of the corresponding pair contact - Map mMapPairIdToContactPairIndex1; + Map mMapPairIdToContactPairIndex1; /// Second map of overlapping pair id to the index of the corresponding pair contact - Map mMapPairIdToContactPairIndex2; + Map mMapPairIdToContactPairIndex2; /// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) - Map* mPreviousMapPairIdToContactPairIndex; + Map* mPreviousMapPairIdToContactPairIndex; /// Pointer to the map of overlappingPairId to the index of contact pair of the current frame /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) - Map* mCurrentMapPairIdToContactPairIndex; + Map* mCurrentMapPairIdToContactPairIndex; /// First list with the contact manifolds List mContactManifolds1; @@ -164,8 +165,8 @@ class CollisionDetectionSystem { #ifdef IS_PROFILING_ACTIVE - /// Pointer to the profiler - Profiler* mProfiler; + /// Pointer to the profiler + Profiler* mProfiler; #endif @@ -175,7 +176,10 @@ class CollisionDetectionSystem { void computeBroadPhase(); /// Compute the middle-phase collision detection - void computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput); + void computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput); + + // Compute the middle-phase collision detection + void computeMiddlePhaseCollisionSnapshot(List& convexPairs, List& concavePairs, NarrowPhaseInput& narrowPhaseInput); /// Compute the narrow-phase collision detection void computeNarrowPhase(); @@ -205,7 +209,7 @@ class CollisionDetectionSystem { bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator); /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies - void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, + void computeConvexVsConcaveMiddlePhase(uint64 pairId, Entity proxyShape1, Entity proxyShape2, MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput); /// Swap the previous and current contacts lists @@ -214,13 +218,13 @@ class CollisionDetectionSystem { /// Convert the potential contact into actual contacts void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, + Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, List* contactPairs, Map>& mapBodyToContactPairs); /// Process the potential contacts after narrow-phase collision detection void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, + Map* mapPairIdToContactPairIndex, List &potentialContactManifolds, List* contactPairs, Map>& mapBodyToContactPairs); @@ -256,10 +260,10 @@ class CollisionDetectionSystem { void processSmoothMeshContacts(OverlappingPair* pair); /// Filter the overlapping pairs to keep only the pairs where a given body is involved - void filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const; + void filterOverlappingPairs(Entity bodyEntity, List& convexPairs, List& concavePairs) const; /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved - void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const; + void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List& convexPairs, List& concavePairs) const; public : @@ -377,12 +381,12 @@ inline void CollisionDetectionSystem::addProxyCollisionShape(ProxyShape* proxySh // Add a pair of bodies that cannot collide with each other inline void CollisionDetectionSystem::addNoCollisionPair(Entity body1Entity, Entity body2Entity) { - mNoCollisionPairs.add(OverlappingPair::computeBodiesIndexPair(body1Entity, body2Entity)); + mNoCollisionPairs.add(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity)); } // Remove a pair of bodies that cannot collide with each other inline void CollisionDetectionSystem::removeNoCollisionPair(Entity body1Entity, Entity body2Entity) { - mNoCollisionPairs.remove(OverlappingPair::computeBodiesIndexPair(body1Entity, body2Entity)); + mNoCollisionPairs.remove(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity)); } // Ask for a collision shape to be tested again during broad-phase. @@ -424,6 +428,7 @@ inline void CollisionDetectionSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; mBroadPhaseSystem.setProfiler(profiler); mCollisionDispatch.setProfiler(profiler); + mOverlappingPairs.setProfiler(profiler); } #endif diff --git a/test/tests/containers/TestList.h b/test/tests/containers/TestList.h index f827fe8a..5fe650da 100644 --- a/test/tests/containers/TestList.h +++ b/test/tests/containers/TestList.h @@ -190,6 +190,22 @@ class TestList : public Test { it = list3.remove(5); rp3d_test((*it) == 6); + list3.clear(); + list3.add(1); + list3.add(2); + list3.add(3); + list3.add(4); + list3.removeAtAndReplaceWithLast(3); + rp3d_test(list3.size() == 3); + rp3d_test(list3[2] == 3); + list3.removeAtAndReplaceWithLast(1); + rp3d_test(list3.size() == 2); + rp3d_test(list3[0] == 2); + rp3d_test(list3[1] == 3); + list3.removeAtAndReplaceWithLast(0); + rp3d_test(list3.size() == 1); + rp3d_test(list3[0] == 3); + // ----- Test addRange() ----- // List list4(mAllocator);