diff --git a/CHANGELOG.md b/CHANGELOG.md index 0c667fb2..eaaa6771 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,12 @@ - Bug [#62](https://github.com/DanielChappuis/reactphysics3d/issues/62) has been fixed. - Bug [#63](https://github.com/DanielChappuis/reactphysics3d/issues/63) has been fixed. +### Removed + + - The CollisionWorld::setCollisionDispatch() method has been removed. In order to use a custom collision + algorithm, you must not get the collision dispatch object with the + CollisionWorld::getCollisionDispatch() method and set a collision algorithm to this object. + ## Version 0.7.0 (May 1, 2018) ### Added diff --git a/CMakeLists.txt b/CMakeLists.txt index dceef275..ce20060a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,7 +84,6 @@ SET (REACTPHYSICS3D_HEADERS "src/collision/broadphase/BroadPhaseAlgorithm.h" "src/collision/broadphase/DynamicAABBTree.h" "src/collision/narrowphase/CollisionDispatch.h" - "src/collision/narrowphase/DefaultCollisionDispatch.h" "src/collision/narrowphase/GJK/VoronoiSimplex.h" "src/collision/narrowphase/GJK/GJKAlgorithm.h" "src/collision/narrowphase/SAT/SATAlgorithm.h" @@ -95,6 +94,8 @@ SET (REACTPHYSICS3D_HEADERS "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/SphereVsSphereNarrowPhaseInputBatch.h" + "src/collision/narrowphase/NarrowPhaseInput.h" "src/collision/shapes/AABB.h" "src/collision/shapes/ConvexShape.h" "src/collision/shapes/ConvexPolyhedronShape.h" @@ -168,7 +169,7 @@ SET (REACTPHYSICS3D_SOURCES "src/body/RigidBody.cpp" "src/collision/broadphase/BroadPhaseAlgorithm.cpp" "src/collision/broadphase/DynamicAABBTree.cpp" - "src/collision/narrowphase/DefaultCollisionDispatch.cpp" + "src/collision/narrowphase/CollisionDispatch.cpp" "src/collision/narrowphase/GJK/VoronoiSimplex.cpp" "src/collision/narrowphase/GJK/GJKAlgorithm.cpp" "src/collision/narrowphase/SAT/SATAlgorithm.cpp" @@ -178,6 +179,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" + "src/collision/narrowphase/SphereVsSphereNarrowPhaseInputBatch.cpp" + "src/collision/narrowphase/NarrowPhaseInput.cpp" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.cpp" "src/collision/shapes/ConvexPolyhedronShape.cpp" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index cfcce432..70e5e0ad 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -49,19 +49,15 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) - : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()), + : mMemoryManager(memoryManager), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this), - mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) { - - // Set the default collision dispatch configuration - setCollisionDispatch(&mDefaultCollisionDispatch); - - // Fill-in the collision detection matrix with algorithms - fillInCollisionMatrix(); + mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false), + mNarrowPhaseInput(mMemoryManager.getPoolAllocator()) { #ifdef IS_PROFILING_ACTIVE mProfiler = nullptr; + mCollisionDispatch.setProfiler(mProfiler); #endif @@ -157,17 +153,21 @@ void CollisionDetection::computeMiddlePhase() { // If both shapes are convex if (isShape1Convex && isShape2Convex) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(shape1->getCollisionShape()->getType(), + shape2->getCollisionShape()->getType()); + // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - mNarrowPhaseInfoBatch.addNarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), + mNarrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), - mMemoryManager.getSingleFrameAllocator()); + algorithmType, mMemoryManager.getSingleFrameAllocator()); } // Concave vs Convex algorithm else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { - computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), mNarrowPhaseInfoBatch); + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), mNarrowPhaseInput); } // Concave vs Concave shape else { @@ -183,7 +183,7 @@ void CollisionDetection::computeMiddlePhase() { // Compute the concave vs convex middle-phase algorithm for a given pair of bodies void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, - NarrowPhaseInfoBatch& narrowPhaseInfoBatch) { + NarrowPhaseInput& narrowPhaseInput) { ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); @@ -207,9 +207,15 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair concaveShape = static_cast(shape1->getCollisionShape()); } + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(convexShape->getType(), + CollisionShapeType::CONVEX_POLYHEDRON); + + assert(algorithmType != NarrowPhaseAlgorithmType::None); + // Set the parameters of the callback object MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, - concaveShape, narrowPhaseInfoBatch, allocator); + concaveShape, narrowPhaseInput, algorithmType, allocator); #ifdef IS_PROFILING_ACTIVE @@ -228,49 +234,92 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair concaveShape->testAllTriangles(middlePhaseCallback, aabb); } +// Execute the narrow-phase collision detection algorithm on batches +bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool stopFirstContactFound, + bool reportContacts, MemoryAllocator& allocator) { + + bool contactFound = false; + + // Get the narrow-phase collision detection algorithms for each kind of collision shapes + SphereVsSphereAlgorithm* sphereVsSphereAlgo = mCollisionDispatch.getSphereVsSphereAlgorithm(); + SphereVsCapsuleAlgorithm* sphereVsCapsuleAlgo = mCollisionDispatch.getSphereVsCapsuleAlgorithm(); + CapsuleVsCapsuleAlgorithm* capsuleVsCapsuleAlgo = mCollisionDispatch.getCapsuleVsCapsuleAlgorithm(); + SphereVsConvexPolyhedronAlgorithm* sphereVsConvexPolyAlgo = mCollisionDispatch.getSphereVsConvexPolyhedronAlgorithm(); + CapsuleVsConvexPolyhedronAlgorithm* capsuleVsConvexPolyAlgo = mCollisionDispatch.getCapsuleVsConvexPolyhedronAlgorithm(); + ConvexPolyhedronVsConvexPolyhedronAlgorithm* convexPolyVsConvexPolyAlgo = mCollisionDispatch.getConvexPolyhedronVsConvexPolyhedronAlgorithm(); + + // get the narrow-phase batches to test for collision + NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); + NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch(); + NarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch(); + NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatch = narrowPhaseInput.getSphereVsConvexPolyhedronBatch(); + NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatch = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch(); + NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch(); + + // Compute the narrow-phase collision detection for each kind of collision shapes + if (sphereVsSphereBatch.getNbObjects() > 0) { + contactFound |= sphereVsSphereAlgo->testCollision(sphereVsSphereBatch, 0, sphereVsSphereBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator); + if (stopFirstContactFound && contactFound) return true; + } + if (sphereVsCapsuleBatch.getNbObjects() > 0) { + contactFound |= sphereVsCapsuleAlgo->testCollision(sphereVsCapsuleBatch, 0, sphereVsCapsuleBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator); + if (stopFirstContactFound && contactFound) return true; + } + if (capsuleVsCapsuleBatch.getNbObjects() > 0) { + contactFound |= capsuleVsCapsuleAlgo->testCollision(capsuleVsCapsuleBatch, 0, capsuleVsCapsuleBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator); + if (stopFirstContactFound && contactFound) return true; + } + if (sphereVsConvexPolyhedronBatch.getNbObjects() > 0) { + contactFound |= sphereVsConvexPolyAlgo->testCollision(sphereVsConvexPolyhedronBatch, 0, sphereVsConvexPolyhedronBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator); + if (stopFirstContactFound && contactFound) return true; + } + if (capsuleVsConvexPolyhedronBatch.getNbObjects() > 0) { + contactFound |= capsuleVsConvexPolyAlgo->testCollision(capsuleVsConvexPolyhedronBatch, 0, capsuleVsConvexPolyhedronBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator); + if (stopFirstContactFound && contactFound) return true; + } + if (convexPolyhedronVsConvexPolyhedronBatch.getNbObjects() > 0) { + contactFound |= convexPolyVsConvexPolyAlgo->testCollision(convexPolyhedronVsConvexPolyhedronBatch, 0, convexPolyhedronVsConvexPolyhedronBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator); + if (stopFirstContactFound && contactFound) return true; + } + + return contactFound; +} + +// Process the potential contacts after narrow-phase collision detection +void CollisionDetection::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo) { + + // get the narrow-phase batches to test for collision + NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); + NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch(); + NarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch(); + NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatch = narrowPhaseInput.getSphereVsConvexPolyhedronBatch(); + NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatch = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch(); + NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch(); + + // Process the potential contacts + processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo); + processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo); + processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo); + processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo); + processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo); + processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo); +} + // Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhase() { RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); - List collidingBatchIndices(mMemoryManager.getSingleFrameAllocator()); + MemoryAllocator& allocator = mMemoryManager.getSingleFrameAllocator(); - // For each narrow phase info to process - for(uint batchIndex=0; batchIndex < mNarrowPhaseInfoBatch.getNbObjects(); batchIndex++) { + // Test the narrow-phase collision detection on the batches to be tested + testNarrowPhaseCollision(mNarrowPhaseInput, false, true, allocator); - assert(mNarrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + // Process all the potential contacts after narrow-phase collision + processAllPotentialContacts(mNarrowPhaseInput, true); - // Select the narrow phase algorithm to use according to the two collision shapes - const CollisionShapeType shape1Type = mNarrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType(); - const CollisionShapeType shape2Type = mNarrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType(); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is no collision algorithm between those two kinds of shapes, skip it - if (narrowPhaseAlgorithm != nullptr) { - - LastFrameCollisionInfo* lastCollisionFrameInfo = mNarrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(mNarrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getSingleFrameAllocator()); - if (mNarrowPhaseInfoBatch.isColliding[batchIndex]) { - - lastCollisionFrameInfo->wasColliding = true; - - collidingBatchIndices.add(batchIndex); - } - else { - lastCollisionFrameInfo->wasColliding = false; - } - - // The previous frame collision info is now valid - lastCollisionFrameInfo->isValid = true; - } - } - - // Convert the potential contact into actual contacts - processAllPotentialContacts(mNarrowPhaseInfoBatch, collidingBatchIndices, mOverlappingPairs); + // Reduce the number of contact points in the manifolds + reduceContactManifolds(mOverlappingPairs); // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); @@ -279,7 +328,7 @@ void CollisionDetection::computeNarrowPhase() { reportAllContacts(); // Clear the list of narrow-phase infos - mNarrowPhaseInfoBatch.clear(); + mNarrowPhaseInput.clear(); } // Allow the broadphase to notify the collision detection about an overlapping pair. @@ -408,24 +457,35 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { } /// Convert the potential contact into actual contacts -void CollisionDetection::processAllPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, const List& collidingBatchIndex, - const OverlappingPairMap& overlappingPairs) { +void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo) { RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); // For each narrow phase info object - for(uint i=0; i < collidingBatchIndex.size(); i++) { + for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { - uint batchIndex = collidingBatchIndex[i]; + if (updateLastFrameInfo) { + narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->wasColliding = narrowPhaseInfoBatch.isColliding[i]; - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0); + // The previous frame collision info is now valid + narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->isValid = true; + } - // Transfer the contact points from the narrow phase info to the overlapping pair - narrowPhaseInfoBatch.overlappingPairs[batchIndex]->addPotentialContactPoints(narrowPhaseInfoBatch, batchIndex); + if (narrowPhaseInfoBatch.isColliding[i]) { - // Remove the contacts points from the narrow phase info object. - narrowPhaseInfoBatch.resetContactPoints(batchIndex); + assert(narrowPhaseInfoBatch.contactPoints[i].size() > 0); + + // Transfer the contact points from the narrow phase info to the overlapping pair + narrowPhaseInfoBatch.overlappingPairs[i]->addPotentialContactPoints(narrowPhaseInfoBatch, i); + + // Remove the contacts points from the narrow phase info object. + narrowPhaseInfoBatch.resetContactPoints(i); + } } +} + +// Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds +void CollisionDetection::reduceContactManifolds(const OverlappingPairMap& overlappingPairs) { // For each overlapping pairs in contact during the narrow-phase for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { @@ -438,6 +498,7 @@ void CollisionDetection::processAllPotentialContacts(NarrowPhaseInfoBatch& narro // Reduce the contact manifolds and contact points if there are too many of them pair->reduceContactManifolds(); } + } // Report contacts for all the colliding overlapping pairs @@ -462,7 +523,7 @@ void CollisionDetection::reportAllContacts() { } // Compute the middle-phase collision detection between two proxy shapes -void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch) { +void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInput& outNarrowPhaseInput) { ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); @@ -477,11 +538,14 @@ void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, // If both shapes are convex if ((isShape1Convex && isShape2Convex)) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(shape1->getCollisionShape()->getType(), + shape2->getCollisionShape()->getType()); // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - outNarrowPhaseInfoBatch.addNarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), + outNarrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), - mMemoryManager.getPoolAllocator()); + algorithmType, mMemoryManager.getPoolAllocator()); } // Concave vs Convex algorithm @@ -489,7 +553,7 @@ void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, // Run the middle-phase collision detection algorithm to find the triangles of the concave // shape we need to use during the narrow-phase collision detection - computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), outNarrowPhaseInfoBatch); + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), outNarrowPhaseInput); } pair->clearObsoleteLastFrameCollisionInfos(); @@ -538,7 +602,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over // Return true if two bodies overlap bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) { - NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); // For each proxy shape proxy shape of the first body ProxyShape* body1ProxyShape = body1->getProxyShapesList(); @@ -560,38 +624,8 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) mMemoryManager.getPoolAllocator(), mWorld->mConfig); // Compute the middle-phase collision detection between the two shapes - computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInfoBatch); + computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInput); - bool isColliding = false; - - // For each narrow-phase info object - for(uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) { - - // If we have not found a collision yet - if (!isColliding) { - - const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, false, mMemoryManager.getPoolAllocator()); - isColliding |= narrowPhaseInfoBatch.isColliding[batchIndex]; - } - } - } - - narrowPhaseInfoBatch.clear(); - - // Return if we have found a narrow-phase collision - if (isColliding) return true; } // Go to the next proxy shape @@ -602,8 +636,13 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) body1ProxyShape = body1ProxyShape->getNext(); } + // Test narrow-phase collision + bool isCollisionFound = testNarrowPhaseCollision(narrowPhaseInput, true, false, mMemoryManager.getPoolAllocator()); + + narrowPhaseInput.clear(); + // No overlap has been found - return false; + return isCollisionFound; } // Report all the bodies that overlap with the body in parameter @@ -613,7 +652,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl assert(overlapCallback != nullptr); Set reportedBodies(mMemoryManager.getPoolAllocator()); - NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); // For each proxy shape proxy shape of the body ProxyShape* bodyProxyShape = body->getProxyShapesList(); @@ -651,38 +690,10 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl mMemoryManager.getPoolAllocator(), mWorld->mConfig); // Compute the middle-phase collision detection between the two shapes - computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInfoBatch); + computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInput); - bool isColliding = false; - - // For each narrow-phase info object - for (uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) { - - // If we have not found a collision yet - if (!isColliding) { - - const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, false, mMemoryManager.getPoolAllocator()); - isColliding |= narrowPhaseInfoBatch.isColliding[batchIndex]; - } - } - } - - narrowPhaseInfoBatch.clear(); - - // Return if we have found a narrow-phase collision - if (isColliding) { + // Test narrow-phase collision + if (testNarrowPhaseCollision(narrowPhaseInput, true, false, mMemoryManager.getPoolAllocator())) { CollisionBody* overlapBody = proxyShape->getBody(); @@ -692,6 +703,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Notify the overlap to the user overlapCallback->notifyOverlap(overlapBody); } + + narrowPhaseInput.clear(); } } @@ -710,8 +723,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body assert(collisionCallback != nullptr); - List collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); - NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); // For each proxy shape proxy shape of the first body @@ -752,7 +764,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body } // Compute the middle-phase collision detection between the two shapes - computeMiddlePhaseForProxyShapes(pair, narrowPhaseInfoBatch); + computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput); } // Go to the next proxy shape @@ -763,31 +775,14 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body body1ProxyShape = body1ProxyShape->getNext(); } - // For each narrow-phase info object - for (uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) { - - const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getPoolAllocator()); - if (narrowPhaseInfoBatch.isColliding[batchIndex]) { - - collidingNarrowPhaseInfos.add(batchIndex); - } - } - } + // Test narrow-phase collision + testNarrowPhaseCollision(narrowPhaseInput, false, true, mMemoryManager.getPoolAllocator()); // Process the potential contacts - processAllPotentialContacts(narrowPhaseInfoBatch, collidingNarrowPhaseInfos, overlappingPairs); + processAllPotentialContacts(narrowPhaseInput, false); + + // Reduce the number of contact points in the manifolds + reduceContactManifolds(overlappingPairs); // For each overlapping pair for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { @@ -812,8 +807,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c assert(callback != nullptr); - List collidingBatchIndices(mMemoryManager.getPoolAllocator()); - NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); // For each proxy shape proxy shape of the body @@ -868,7 +862,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c } // Compute the middle-phase collision detection between the two shapes - computeMiddlePhaseForProxyShapes(pair, narrowPhaseInfoBatch); + computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput); } } @@ -881,31 +875,14 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c } } - // For each narrow-phase info object - for (uint batchIndex = 0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) { - - const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getPoolAllocator()); - if (narrowPhaseInfoBatch.isColliding[batchIndex]) { - - collidingBatchIndices.add(batchIndex); - } - } - } + // Test narrow-phase collision + testNarrowPhaseCollision(narrowPhaseInput, false, true, mMemoryManager.getPoolAllocator()); // Process the potential contacts - processAllPotentialContacts(narrowPhaseInfoBatch, collidingBatchIndices, overlappingPairs); + processAllPotentialContacts(narrowPhaseInput, false); + + // Reduce the number of contact points in the manifolds + reduceContactManifolds(overlappingPairs); // For each overlapping pair for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { @@ -933,8 +910,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Compute the broad-phase collision detection computeBroadPhase(); - List collidingBatchIndices(mMemoryManager.getPoolAllocator()); - NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()); + NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator()); OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); // For each possible collision pair of bodies @@ -974,35 +950,18 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Compute the middle-phase collision detection between the two shapes - computeMiddlePhaseForProxyShapes(pair, narrowPhaseInfoBatch); + computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput); } } - // For each narrow-phase info object - for (uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) { - - const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getPoolAllocator()); - if (narrowPhaseInfoBatch.isColliding[batchIndex]) { - - collidingBatchIndices.add(batchIndex); - } - } - } + // Test narrow-phase collision + testNarrowPhaseCollision(narrowPhaseInput, false, true, mMemoryManager.getPoolAllocator()); // Process the potential contacts - processAllPotentialContacts(narrowPhaseInfoBatch, collidingBatchIndices, overlappingPairs); + processAllPotentialContacts(narrowPhaseInput, false); + + // Reduce the number of contact points in the manifolds + reduceContactManifolds(overlappingPairs); // For each overlapping pair for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { @@ -1022,17 +981,6 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { } } -// Fill-in the collision detection matrix -void CollisionDetection::fillInCollisionMatrix() { - - // For each possible type of collision shape - for (int i=0; iselectAlgorithm(i, j); - } - } -} - // Return the world event listener EventListener* CollisionDetection::getWorldEventListener() { return mWorld->mEventListener; diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 31f6de81..b10c83e3 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -31,8 +31,8 @@ #include "broadphase/BroadPhaseAlgorithm.h" #include "collision/shapes/CollisionShape.h" #include "engine/OverlappingPair.h" -#include "collision/narrowphase/DefaultCollisionDispatch.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInput.h" +#include "collision/narrowphase/CollisionDispatch.h" #include "containers/Map.h" #include "containers/Set.h" @@ -68,20 +68,11 @@ class CollisionDetection { MemoryManager& mMemoryManager; /// Collision Detection Dispatch configuration - CollisionDispatch* mCollisionDispatch; - - /// Default collision dispatch configuration - DefaultCollisionDispatch mDefaultCollisionDispatch; - - /// Collision detection matrix (algorithms to use) - NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; + CollisionDispatch mCollisionDispatch; /// Pointer to the physics world CollisionWorld* mWorld; - /// List of narrow phase infos - NarrowPhaseInfoBatch mNarrowPhaseInfoBatch; - /// Broad-phase overlapping pairs OverlappingPairMap mOverlappingPairs; @@ -94,6 +85,9 @@ class CollisionDetection { /// True if some collision shapes have been added previously bool mIsCollisionShapesAdded; + /// Narrow-phase collision detection input + NarrowPhaseInput mNarrowPhaseInput; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -112,31 +106,33 @@ class CollisionDetection { /// Compute the narrow-phase collision detection void computeNarrowPhase(); + /// Execute the narrow-phase collision detection algorithm on batches + bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool stopFirstContactFound, + bool reportContacts, MemoryAllocator& allocator); + /// Add a contact manifold to the linked list of contact manifolds of the two bodies /// involved in the corresponding contact. void addContactManifoldToBody(OverlappingPair* pair); - /// Fill-in the collision detection matrix - void fillInCollisionMatrix(); - - /// Return the corresponding narrow-phase algorithm - NarrowPhaseAlgorithm* selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, - const CollisionShapeType& shape2Type) const; - /// Add all the contact manifold of colliding pairs to their bodies void addAllContactManifoldsToBodies(); /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, - NarrowPhaseInfoBatch& narrowPhaseInfoBatch); + NarrowPhaseInput& narrowPhaseInput); /// Compute the middle-phase collision detection between two proxy shapes - void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch); + void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInput& outNarrowPhaseInput); /// Convert the potential contact into actual contacts - void processAllPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, - const List& collidingBatchIndex, - const OverlappingPairMap& overlappingPairs); + void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, + bool updateLastFrameInfo); + + /// Process the potential contacts after narrow-phase collision detection + void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo); + + /// Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds + void reduceContactManifolds(const OverlappingPairMap& overlappingPairs); /// Report contacts for all the colliding overlapping pairs void reportAllContacts(); @@ -161,7 +157,7 @@ class CollisionDetection { CollisionDetection& operator=(const CollisionDetection& collisionDetection) = delete; /// Set the collision dispatch configuration - void setCollisionDispatch(CollisionDispatch* collisionDispatch); + CollisionDispatch& getCollisionDispatch(); /// Add a proxy collision shape to the collision detection void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); @@ -235,20 +231,9 @@ class CollisionDetection { friend class ConvexMeshShape; }; -// Set the collision dispatch configuration -inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - mCollisionDispatch = collisionDispatch; - - // Fill-in the collision matrix with the new algorithms to use - fillInCollisionMatrix(); - -#ifdef IS_PROFILING_ACTIVE - - // Set the profiler - mCollisionDispatch->setProfiler(mProfiler); - -#endif - +// Return a reference to the collision dispatch configuration +inline CollisionDispatch& CollisionDetection::getCollisionDispatch() { + return mCollisionDispatch; } // Add a body to the collision detection @@ -289,24 +274,6 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } -// Return the corresponding narrow-phase algorithm -inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, - const CollisionShapeType& shape2Type) const { - - uint shape1Index = static_cast(shape1Type); - uint shape2Index = static_cast(shape2Type); - - // Swap the shape types if necessary - if (shape1Index > shape2Index) { - const uint tempIndex = shape1Index; - shape1Index = shape2Index; - shape2Index = tempIndex; - } - - assert(shape1Index <= shape2Index); - - return mCollisionMatrix[shape1Index][shape2Index]; -} // Return a pointer to the world inline CollisionWorld* CollisionDetection::getWorld() { @@ -324,7 +291,7 @@ inline MemoryManager& CollisionDetection::getMemoryManager() const { inline void CollisionDetection::setProfiler(Profiler* profiler) { mProfiler = profiler; mBroadPhaseAlgorithm.setProfiler(profiler); - mCollisionDispatch->setProfiler(profiler); + mCollisionDispatch.setProfiler(profiler); } #endif diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index 9930bc98..c363ef1b 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -27,7 +27,7 @@ #include "collision/MiddlePhaseTriangleCallback.h" #include "engine/OverlappingPair.h" #include "collision/shapes/TriangleShape.h" -#include "collision/NarrowPhaseInfoBatch.h" +#include "collision/narrowphase/NarrowPhaseInput.h" using namespace reactphysics3d; @@ -51,8 +51,9 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape; // Create a narrow phase info for the narrow-phase collision detection - mOutNarrowPhaseInfoBatch.addNarrowPhaseInfo(mOverlappingPair, - isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape, - isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(), - shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), mAllocator); + mOutNarrowPhaseInput.addNarrowPhaseTest(mOverlappingPair, + isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape, + isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(), + shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), + mNarrowPhaseAlgorithmType, mAllocator); } diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h index abe624fd..1c417a60 100644 --- a/src/collision/MiddlePhaseTriangleCallback.h +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -41,7 +41,8 @@ class NarrowPhaseAlgorithm; class ProxyShape; class MemoryAllocator; class Profiler; -struct NarrowPhaseInfoBatch; +class NarrowPhaseInput; +enum class NarrowPhaseAlgorithmType; struct Vector3; // Class ConvexVsTriangleCallback @@ -66,8 +67,11 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { /// Pointer to the concave collision shape const ConcaveShape* mConcaveShape; - /// Reference to the narrow phase info batch - NarrowPhaseInfoBatch& mOutNarrowPhaseInfoBatch; + /// Reference to the narrow phase input + NarrowPhaseInput& mOutNarrowPhaseInput; + + /// Type of narrow-phase algorithm to use + NarrowPhaseAlgorithmType mNarrowPhaseAlgorithmType; /// Reference to the single-frame memory allocator MemoryAllocator& mAllocator; @@ -85,11 +89,13 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, ProxyShape* concaveProxyShape, ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, - NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch, + NarrowPhaseInput& outNarrowPhaseInput, + NarrowPhaseAlgorithmType algorithmType, MemoryAllocator& allocator) :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), - mOutNarrowPhaseInfoBatch(outNarrowPhaseInfoBatch), mAllocator(allocator) { + mOutNarrowPhaseInput(outNarrowPhaseInput), mNarrowPhaseAlgorithmType(algorithmType), + mAllocator(allocator) { } diff --git a/src/collision/NarrowPhaseInfoBatch.cpp b/src/collision/NarrowPhaseInfoBatch.cpp index 3888fc29..1e8807eb 100644 --- a/src/collision/NarrowPhaseInfoBatch.cpp +++ b/src/collision/NarrowPhaseInfoBatch.cpp @@ -35,7 +35,8 @@ using namespace reactphysics3d; NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator) : mMemoryAllocator(allocator), overlappingPairs(allocator), collisionShapes1(allocator), collisionShapes2(allocator), shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator), - isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator), lastFrameCollisionInfos(allocator) { + isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator), + lastFrameCollisionInfos(allocator) { } @@ -59,9 +60,8 @@ 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) - pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); - - lastFrameCollisionInfos.add(pair->getLastFrameCollisionInfo(shape1->getId(), shape2->getId())); + LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + lastFrameCollisionInfos.add(lastFrameInfo); } // Add a new contact point diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index f7075fcd..3412a4ca 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -34,11 +34,15 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -void CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, - MemoryAllocator& memoryAllocator) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, + bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { + bool isCollisionFound = false; + for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { + assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE); @@ -151,6 +155,10 @@ void CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI } narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } } @@ -227,6 +235,12 @@ void CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI } narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } } } + + return isCollisionFound; } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 3e1a5871..8612c9ed 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -67,8 +67,8 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) override; }; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 54797d86..e611109b 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -39,11 +39,13 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a capsule and a polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -void CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts, + bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { + bool isCollisionFound = false; + // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; SATAlgorithm satAlgorithm(memoryAllocator); @@ -60,7 +62,6 @@ void CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults, reportContacts); assert(gjkResults.size() == batchNbItems); - uint resultIndex = 0; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { // Get the last frame collision info @@ -75,7 +76,7 @@ void CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE); // If we have found a contact point inside the margins (shallow penetration) - if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { if (reportContacts) { @@ -162,21 +163,32 @@ void CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar lastFrameCollisionInfo->wasUsingSAT = false; lastFrameCollisionInfo->wasUsingGJK = false; - // Return true + // Colision found narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } // If we have overlap even without the margins (deep penetration) - if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) { + if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, reportContacts); lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingSAT = true; - } - resultIndex++; + if (narrowPhaseInfoBatch.isColliding[batchIndex]) { + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } + } + } } + + return isCollisionFound; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index 3ccfc932..c69225c8 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -70,8 +70,8 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a capsule and a polyhedron - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) override; }; diff --git a/src/collision/narrowphase/CollisionDispatch.cpp b/src/collision/narrowphase/CollisionDispatch.cpp new file mode 100644 index 00000000..f1a4c607 --- /dev/null +++ b/src/collision/narrowphase/CollisionDispatch.cpp @@ -0,0 +1,215 @@ +/******************************************************************************** +* 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 "CollisionDispatch.h" + +using namespace reactphysics3d; + +// Constructor +CollisionDispatch::CollisionDispatch(MemoryAllocator& allocator) : mAllocator(allocator) { + + // Create the default narrow-phase algorithms + mSphereVsSphereAlgorithm = new (allocator.allocate(sizeof(SphereVsSphereAlgorithm))) SphereVsSphereAlgorithm(); + mSphereVsCapsuleAlgorithm = new (allocator.allocate(sizeof(SphereVsCapsuleAlgorithm))) SphereVsCapsuleAlgorithm(); + mCapsuleVsCapsuleAlgorithm = new (allocator.allocate(sizeof(CapsuleVsCapsuleAlgorithm))) CapsuleVsCapsuleAlgorithm(); + mSphereVsConvexPolyhedronAlgorithm = new (allocator.allocate(sizeof(SphereVsConvexPolyhedronAlgorithm))) SphereVsConvexPolyhedronAlgorithm(); + mCapsuleVsConvexPolyhedronAlgorithm = new (allocator.allocate(sizeof(CapsuleVsConvexPolyhedronAlgorithm))) CapsuleVsConvexPolyhedronAlgorithm(); + mConvexPolyhedronVsConvexPolyhedronAlgorithm = new (allocator.allocate(sizeof(ConvexPolyhedronVsConvexPolyhedronAlgorithm))) ConvexPolyhedronVsConvexPolyhedronAlgorithm(); + + // Fill in the collision matrix + fillInCollisionMatrix(); +} + +// Destructor +CollisionDispatch::~CollisionDispatch() { + + // Release allocated memory + if (mIsSphereVsSphereDefault) { + mAllocator.release(mSphereVsSphereAlgorithm, sizeof(SphereVsSphereAlgorithm)); + } + if (mIsSphereVsCapsuleDefault) { + mAllocator.release(mSphereVsCapsuleAlgorithm, sizeof(SphereVsCapsuleAlgorithm)); + } + if (mIsCapsuleVsCapsuleDefault) { + mAllocator.release(mCapsuleVsCapsuleAlgorithm, sizeof(CapsuleVsCapsuleAlgorithm)); + } + if (mIsSphereVsConvexPolyhedronDefault) { + mAllocator.release(mSphereVsConvexPolyhedronAlgorithm, sizeof(SphereVsConvexPolyhedronAlgorithm)); + } + if (mIsCapsuleVsConvexPolyhedronDefault) { + mAllocator.release(mCapsuleVsConvexPolyhedronAlgorithm, sizeof(CapsuleVsConvexPolyhedronAlgorithm)); + } + if (mIsConvexPolyhedronVsConvexPolyhedronDefault) { + mAllocator.release(mConvexPolyhedronVsConvexPolyhedronAlgorithm, sizeof(ConvexPolyhedronVsConvexPolyhedronAlgorithm)); + } +} + +// Select and return the narrow-phase collision detection algorithm to +// use between two types of collision shapes. +NarrowPhaseAlgorithmType CollisionDispatch::selectAlgorithm(int type1, int type2) { + + CollisionShapeType shape1Type = static_cast(type1); + CollisionShapeType shape2Type = static_cast(type2); + + if (type1 > type2) { + return NarrowPhaseAlgorithmType::None; + } + // Sphere vs Sphere algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { + return NarrowPhaseAlgorithmType::SphereVsSphere; + } + // Sphere vs Capsule algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) { + return NarrowPhaseAlgorithmType::SphereVsCapsule; + } + // Capsule vs Capsule algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) { + return NarrowPhaseAlgorithmType::CapsuleVsCapsule; + } + // Sphere vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron; + } + // Capsule vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return NarrowPhaseAlgorithmType::CapsuleVsConvexPolyhedron; + } + // Convex Polyhedron vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON && + shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return NarrowPhaseAlgorithmType::ConvexPolyhedronVsConvexPolyhedron; + } + + return NarrowPhaseAlgorithmType::None; +} + +// Set the Sphere vs Sphere narrow-phase collision detection algorithm +void CollisionDispatch::setSphereVsSphereAlgorithm(SphereVsSphereAlgorithm* algorithm) { + + if (mIsSphereVsSphereDefault) { + mAllocator.release(mSphereVsSphereAlgorithm, sizeof(SphereVsSphereAlgorithm)); + mIsSphereVsSphereDefault = false; + } + + mSphereVsSphereAlgorithm = algorithm; + + fillInCollisionMatrix(); +} + +// Set the Sphere vs Capsule narrow-phase collision detection algorithm +void CollisionDispatch::setSphereVsCapsuleAlgorithm(SphereVsCapsuleAlgorithm* algorithm) { + + if (mIsSphereVsCapsuleDefault) { + mAllocator.release(mSphereVsCapsuleAlgorithm, sizeof(SphereVsCapsuleAlgorithm)); + mIsSphereVsCapsuleDefault = false; + } + + mSphereVsCapsuleAlgorithm = algorithm; + + fillInCollisionMatrix(); +} + +// Set the Capsule vs Capsule narrow-phase collision detection algorithm +void CollisionDispatch::setCapsuleVsCapsuleAlgorithm(CapsuleVsCapsuleAlgorithm* algorithm) { + + if (mIsCapsuleVsCapsuleDefault) { + mAllocator.release(mCapsuleVsCapsuleAlgorithm, sizeof(CapsuleVsCapsuleAlgorithm)); + mIsCapsuleVsCapsuleDefault = false; + } + + mCapsuleVsCapsuleAlgorithm = algorithm; + + fillInCollisionMatrix(); +} + +// Set the Sphere vs Convex Polyhedron narrow-phase collision detection algorithm +void CollisionDispatch::setSphereVsConvexPolyhedronAlgorithm(SphereVsConvexPolyhedronAlgorithm* algorithm) { + + if (mIsSphereVsConvexPolyhedronDefault) { + mAllocator.release(mSphereVsConvexPolyhedronAlgorithm, sizeof(SphereVsConvexPolyhedronAlgorithm)); + mIsSphereVsConvexPolyhedronDefault = false; + } + + mSphereVsConvexPolyhedronAlgorithm = algorithm; + + fillInCollisionMatrix(); +} + +// Set the Capsule vs Convex Polyhedron narrow-phase collision detection algorithm +void CollisionDispatch::setCapsuleVsConvexPolyhedronAlgorithm(CapsuleVsConvexPolyhedronAlgorithm* algorithm) { + + if (mIsCapsuleVsConvexPolyhedronDefault) { + mAllocator.release(mCapsuleVsConvexPolyhedronAlgorithm, sizeof(CapsuleVsConvexPolyhedronAlgorithm)); + mIsCapsuleVsConvexPolyhedronDefault = false; + } + + mCapsuleVsConvexPolyhedronAlgorithm = algorithm; + + fillInCollisionMatrix(); +} + +// Set the Convex Polyhedron vs Convex Polyhedron narrow-phase collision detection algorithm +void CollisionDispatch::setConvexPolyhedronVsConvexPolyhedronAlgorithm(ConvexPolyhedronVsConvexPolyhedronAlgorithm* algorithm) { + + if (mIsConvexPolyhedronVsConvexPolyhedronDefault) { + mAllocator.release(mConvexPolyhedronVsConvexPolyhedronAlgorithm, sizeof(ConvexPolyhedronVsConvexPolyhedronAlgorithm)); + mIsConvexPolyhedronVsConvexPolyhedronDefault = false; + } + + mConvexPolyhedronVsConvexPolyhedronAlgorithm = algorithm; + + fillInCollisionMatrix(); +} + + +// Fill-in the collision detection matrix +void CollisionDispatch::fillInCollisionMatrix() { + + // For each possible type of collision shape + for (int i=0; i(shape1Type); + uint shape2Index = static_cast(shape2Type); + + // Swap the shape types if necessary + if (shape1Index > shape2Index) { + return mCollisionMatrix[shape2Index][shape1Index]; + } + + return mCollisionMatrix[shape1Index][shape2Index]; +} + + + diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 363d25b3..58309c00 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -27,57 +27,186 @@ #define REACTPHYSICS3D_COLLISION_DISPATCH_H // Libraries +#include "CollisionDispatch.h" +#include "SphereVsSphereAlgorithm.h" +#include "SphereVsConvexPolyhedronAlgorithm.h" +#include "SphereVsCapsuleAlgorithm.h" +#include "CapsuleVsCapsuleAlgorithm.h" +#include "CapsuleVsConvexPolyhedronAlgorithm.h" +#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" +#include "collision/shapes/CollisionShape.h" namespace reactphysics3d { -// Declarations -class NarrowPhaseAlgorithm; -class Profiler; +/// Enumeration for the type of narrow-phase +/// collision detection algorithm +enum class NarrowPhaseAlgorithmType { + None, + SphereVsSphere, + SphereVsCapsule, + CapsuleVsCapsule, + SphereVsConvexPolyhedron, + CapsuleVsConvexPolyhedron, + ConvexPolyhedronVsConvexPolyhedron +}; // Class CollisionDispatch /** - * This is the abstract base class for dispatching the narrow-phase - * collision detection algorithm. Collision dispatching decides which collision + * This is the collision dispatch configuration use in ReactPhysics3D. + * Collision dispatching decides which collision * algorithm to use given two types of proxy collision shapes. */ class CollisionDispatch { protected: -#ifdef IS_PROFILING_ACTIVE + /// Memory allocator + MemoryAllocator& mAllocator; - /// Pointer to the profiler - Profiler* mProfiler; + /// True if the sphere vs sphere algorithm is the default one + bool mIsSphereVsSphereDefault = true; -#endif + /// True if the capsule vs capsule algorithm is the default one + bool mIsCapsuleVsCapsuleDefault = true; + + /// True if the sphere vs capsule algorithm is the default one + bool mIsSphereVsCapsuleDefault = true; + + /// True if the sphere vs convex polyhedron algorithm is the default one + bool mIsSphereVsConvexPolyhedronDefault = true; + + /// True if the capsule vs convex polyhedron algorithm is the default one + bool mIsCapsuleVsConvexPolyhedronDefault = true; + + /// True if the convex polyhedron vs convex polyhedron algorithm is the default one + bool mIsConvexPolyhedronVsConvexPolyhedronDefault = true; + + /// Sphere vs Sphere collision algorithm + SphereVsSphereAlgorithm* mSphereVsSphereAlgorithm; + + /// Capsule vs Capsule collision algorithm + CapsuleVsCapsuleAlgorithm* mCapsuleVsCapsuleAlgorithm; + + /// Sphere vs Capsule collision algorithm + SphereVsCapsuleAlgorithm* mSphereVsCapsuleAlgorithm; + + /// Sphere vs Convex Polyhedron collision algorithm + SphereVsConvexPolyhedronAlgorithm* mSphereVsConvexPolyhedronAlgorithm; + + /// Capsule vs Convex Polyhedron collision algorithm + CapsuleVsConvexPolyhedronAlgorithm* mCapsuleVsConvexPolyhedronAlgorithm; + + /// Convex Polyhedron vs Convex Polyhedron collision algorithm + ConvexPolyhedronVsConvexPolyhedronAlgorithm* mConvexPolyhedronVsConvexPolyhedronAlgorithm; + + /// Collision detection matrix (algorithms to use) + NarrowPhaseAlgorithmType mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; + + /// Select and return the narrow-phase collision detection algorithm to + /// use between two types of collision shapes. + NarrowPhaseAlgorithmType selectAlgorithm(int type1, int type2); public: /// Constructor - CollisionDispatch() = default; + CollisionDispatch(MemoryAllocator& allocator); /// Destructor - virtual ~CollisionDispatch() = default; + ~CollisionDispatch(); - /// Select and return the narrow-phase collision detection algorithm to - /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type)=0; + /// Set the Sphere vs Sphere narrow-phase collision detection algorithm + void setSphereVsSphereAlgorithm(SphereVsSphereAlgorithm* algorithm); + + /// Get the Sphere vs Sphere narrow-phase collision detection algorithm + SphereVsSphereAlgorithm* getSphereVsSphereAlgorithm(); + + /// Set the Sphere vs Capsule narrow-phase collision detection algorithm + void setSphereVsCapsuleAlgorithm(SphereVsCapsuleAlgorithm* algorithm); + + /// Get the Sphere vs Capsule narrow-phase collision detection algorithm + SphereVsCapsuleAlgorithm* getSphereVsCapsuleAlgorithm(); + + /// Set the Capsule vs Capsule narrow-phase collision detection algorithm + void setCapsuleVsCapsuleAlgorithm(CapsuleVsCapsuleAlgorithm* algorithm); + + /// Get the Capsule vs Capsule narrow-phase collision detection algorithm + CapsuleVsCapsuleAlgorithm* getCapsuleVsCapsuleAlgorithm(); + + /// Set the Sphere vs Convex Polyhedron narrow-phase collision detection algorithm + void setSphereVsConvexPolyhedronAlgorithm(SphereVsConvexPolyhedronAlgorithm* algorithm); + + /// Get the Sphere vs Convex Polyhedron narrow-phase collision detection algorithm + SphereVsConvexPolyhedronAlgorithm* getSphereVsConvexPolyhedronAlgorithm(); + + /// Set the Capsule vs Convex Polyhedron narrow-phase collision detection algorithm + void setCapsuleVsConvexPolyhedronAlgorithm(CapsuleVsConvexPolyhedronAlgorithm* algorithm); + + /// Get the Capsule vs Convex Polyhedron narrow-phase collision detection algorithm + CapsuleVsConvexPolyhedronAlgorithm* getCapsuleVsConvexPolyhedronAlgorithm(); + + /// Set the Convex Polyhedron vs Convex Polyhedron narrow-phase collision detection algorithm + void setConvexPolyhedronVsConvexPolyhedronAlgorithm(ConvexPolyhedronVsConvexPolyhedronAlgorithm* algorithm); + + /// Get the Convex Polyhedron vs Convex Polyhedron narrow-phase collision detection algorithm + ConvexPolyhedronVsConvexPolyhedronAlgorithm* getConvexPolyhedronVsConvexPolyhedronAlgorithm(); + + /// Fill-in the collision detection matrix + void fillInCollisionMatrix(); + + /// Return the corresponding narrow-phase algorithm type to use for two collision shapes + NarrowPhaseAlgorithmType selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, + const CollisionShapeType& shape2Type) const; #ifdef IS_PROFILING_ACTIVE /// Set the profiler - virtual void setProfiler(Profiler* profiler); + void setProfiler(Profiler* profiler); #endif }; +// Get the Sphere vs Sphere narrow-phase collision detection algorithm +inline SphereVsSphereAlgorithm* CollisionDispatch::getSphereVsSphereAlgorithm() { + return mSphereVsSphereAlgorithm; +} + +// Get the Sphere vs Capsule narrow-phase collision detection algorithm +inline SphereVsCapsuleAlgorithm* CollisionDispatch::getSphereVsCapsuleAlgorithm() { + return mSphereVsCapsuleAlgorithm; +} + +// Get the Capsule vs Capsule narrow-phase collision detection algorithm +inline CapsuleVsCapsuleAlgorithm* CollisionDispatch::getCapsuleVsCapsuleAlgorithm() { + return mCapsuleVsCapsuleAlgorithm; +} + +// Get the Sphere vs Convex Polyhedron narrow-phase collision detection algorithm +inline SphereVsConvexPolyhedronAlgorithm* CollisionDispatch::getSphereVsConvexPolyhedronAlgorithm() { + return mSphereVsConvexPolyhedronAlgorithm; +} + +// Get the Capsule vs Convex Polyhedron narrow-phase collision detection algorithm +inline CapsuleVsConvexPolyhedronAlgorithm* CollisionDispatch::getCapsuleVsConvexPolyhedronAlgorithm() { + return mCapsuleVsConvexPolyhedronAlgorithm; +} + +// Get the Convex Polyhedron vs Convex Polyhedron narrow-phase collision detection algorithm +inline ConvexPolyhedronVsConvexPolyhedronAlgorithm* CollisionDispatch::getConvexPolyhedronVsConvexPolyhedronAlgorithm() { + return mConvexPolyhedronVsConvexPolyhedronAlgorithm; +} + #ifdef IS_PROFILING_ACTIVE // Set the profiler inline void CollisionDispatch::setProfiler(Profiler* profiler) { - mProfiler = profiler; + mSphereVsSphereAlgorithm->setProfiler(profiler); + mCapsuleVsCapsuleAlgorithm->setProfiler(profiler); + mSphereVsCapsuleAlgorithm->setProfiler(profiler); + mSphereVsConvexPolyhedronAlgorithm->setProfiler(profiler); + mCapsuleVsConvexPolyhedronAlgorithm->setProfiler(profiler); + mConvexPolyhedronVsConvexPolyhedronAlgorithm->setProfiler(profiler); } #endif @@ -86,3 +215,5 @@ inline void CollisionDispatch::setProfiler(Profiler* profiler) { #endif + + diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index c20d0390..fe47ac3e 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -35,9 +35,9 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -void ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts, + bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { // Run the SAT algorithm to find the separating axis and compute contact point @@ -49,7 +49,8 @@ void ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB #endif - satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, reportContacts); + bool isCollisionFound = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfoBatch, batchStartIndex, + batchNbItems, reportContacts, stopFirstContactFound); for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { @@ -58,5 +59,11 @@ void ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB lastFrameCollisionInfo->wasUsingSAT = true; lastFrameCollisionInfo->wasUsingGJK = false; + + if (isCollisionFound && stopFirstContactFound) { + return isCollisionFound; + } } + + return isCollisionFound; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index 9cf47689..b63fb53e 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -65,8 +65,8 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two convex polyhedra - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) override; }; diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp deleted file mode 100644 index 1c71aa50..00000000 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ /dev/null @@ -1,69 +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 "DefaultCollisionDispatch.h" -#include "collision/shapes/CollisionShape.h" - -using namespace reactphysics3d; - -// Select and return the narrow-phase collision detection algorithm to -// use between two types of collision shapes. -NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { - - CollisionShapeType shape1Type = static_cast(type1); - CollisionShapeType shape2Type = static_cast(type2); - - if (type1 > type2) { - return nullptr; - } - // Sphere vs Sphere algorithm - if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { - return &mSphereVsSphereAlgorithm; - } - // Sphere vs Capsule algorithm - if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) { - return &mSphereVsCapsuleAlgorithm; - } - // Capsule vs Capsule algorithm - if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) { - return &mCapsuleVsCapsuleAlgorithm; - } - // Sphere vs Convex Polyhedron algorithm - if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { - return &mSphereVsConvexPolyhedronAlgorithm; - } - // Capsule vs Convex Polyhedron algorithm - if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { - return &mCapsuleVsConvexPolyhedronAlgorithm; - } - // Convex Polyhedron vs Convex Polyhedron algorithm - if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON && - shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { - return &mConvexPolyhedronVsConvexPolyhedronAlgorithm; - } - - return nullptr; -} diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h deleted file mode 100644 index 348d474a..00000000 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ /dev/null @@ -1,111 +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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H -#define REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H - -// Libraries -#include "CollisionDispatch.h" -#include "SphereVsSphereAlgorithm.h" -#include "SphereVsConvexPolyhedronAlgorithm.h" -#include "SphereVsCapsuleAlgorithm.h" -#include "CapsuleVsCapsuleAlgorithm.h" -#include "CapsuleVsConvexPolyhedronAlgorithm.h" -#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" - -namespace reactphysics3d { - -// Class DefaultCollisionDispatch -/** - * This is the default collision dispatch configuration use in ReactPhysics3D. - * Collision dispatching decides which collision - * algorithm to use given two types of proxy collision shapes. - */ -class DefaultCollisionDispatch : public CollisionDispatch { - - protected: - - /// Sphere vs Sphere collision algorithm - SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - - /// Capsule vs Capsule collision algorithm - CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; - - /// Sphere vs Capsule collision algorithm - SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm; - - /// Sphere vs Convex Polyhedron collision algorithm - SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; - - /// Capsule vs Convex Polyhedron collision algorithm - CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm; - - /// Convex Polyhedron vs Convex Polyhedron collision algorithm - ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm; - - public: - - /// Constructor - DefaultCollisionDispatch() = default; - - /// Destructor - virtual ~DefaultCollisionDispatch() override = default; - - /// Select and return the narrow-phase collision detection algorithm to - /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override; - -#ifdef IS_PROFILING_ACTIVE - - /// Set the profiler - virtual void setProfiler(Profiler* profiler) override; - -#endif - -}; - -#ifdef IS_PROFILING_ACTIVE - -// Set the profiler -inline void DefaultCollisionDispatch::setProfiler(Profiler* profiler) { - - CollisionDispatch::setProfiler(profiler); - - mSphereVsSphereAlgorithm.setProfiler(profiler); - mCapsuleVsCapsuleAlgorithm.setProfiler(profiler); - mSphereVsCapsuleAlgorithm.setProfiler(profiler); - mSphereVsConvexPolyhedronAlgorithm.setProfiler(profiler); - mCapsuleVsConvexPolyhedronAlgorithm.setProfiler(profiler); - mConvexPolyhedronVsConvexPolyhedronAlgorithm.setProfiler(profiler); -} - -#endif - -} - -#endif - - - diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 64b9ac87..bbda9a62 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -127,6 +127,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin lastFrameCollisionInfo->gjkSeparatingAxis = v; // No intersection, we return + assert(gjkResults.size() == batchIndex); gjkResults.add(GJKResult::SEPARATED); noIntersection = true; break; @@ -201,12 +202,14 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin // If the penetration depth is negative (due too numerical errors), there is no contact if (penetrationDepth <= decimal(0.0)) { + assert(gjkResults.size() == batchIndex); gjkResults.add(GJKResult::SEPARATED); continue; } // Do not generate a contact point with zero normal length if (normal.lengthSquare() < MACHINE_EPSILON) { + assert(gjkResults.size() == batchIndex); gjkResults.add(GJKResult::SEPARATED); continue; } @@ -221,10 +224,13 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, pA, pB); } + assert(gjkResults.size() == batchIndex); gjkResults.add(GJKResult::COLLIDE_IN_MARGIN); + continue; } + assert(gjkResults.size() == batchIndex); gjkResults.add(GJKResult::INTERPENETRATE); } } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 6d376172..608651c9 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -59,6 +59,7 @@ class NarrowPhaseCallback { }; +// TODO DOD : Try to delete this class // Class NarrowPhaseAlgorithm /** * This abstract class is the base class for a narrow-phase collision @@ -71,6 +72,9 @@ class NarrowPhaseAlgorithm { // -------------------- Attributes -------------------- // + // Id of the algorithm (computed by hasing the algorithm name) + size_t mId; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -80,6 +84,7 @@ class NarrowPhaseAlgorithm { public : + // -------------------- Methods -------------------- // /// Constructor @@ -95,8 +100,8 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volumes collide - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator)=0; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp new file mode 100644 index 00000000..da08213c --- /dev/null +++ b/src/collision/narrowphase/NarrowPhaseInput.cpp @@ -0,0 +1,80 @@ +/******************************************************************************** +* 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 "collision/narrowphase/NarrowPhaseInput.h" +#include "collision/narrowphase/CollisionDispatch.h" + +using namespace reactphysics3d; + +/// Constructor +NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator) + :mSphereVsSphereBatch(allocator), mSphereVsCapsuleBatch(allocator), mCapsuleVsCapsuleBatch(allocator), + mSphereVsConvexPolyhedronBatch(allocator), mCapsuleVsConvexPolyhedronBatch(allocator), + mConvexPolyhedronVsConvexPolyhedronBatch(allocator) { + +} + +// Add shapes to be tested during narrow-phase collision detection into the batch +void NarrowPhaseInput::addNarrowPhaseTest(OverlappingPair* pair, 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, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::SphereVsCapsule: + mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::CapsuleVsCapsule: + mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron: + mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::CapsuleVsConvexPolyhedron: + mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::ConvexPolyhedronVsConvexPolyhedron: + mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::None: + // Must never happen + assert(false); + break; + } +} + +// Clear +void NarrowPhaseInput::clear() { + + mSphereVsSphereBatch.clear(); + mSphereVsCapsuleBatch.clear(); + mCapsuleVsCapsuleBatch.clear(); + mSphereVsConvexPolyhedronBatch.clear(); + mCapsuleVsConvexPolyhedronBatch.clear(); + mConvexPolyhedronVsConvexPolyhedronBatch.clear(); +} diff --git a/src/collision/narrowphase/NarrowPhaseInput.h b/src/collision/narrowphase/NarrowPhaseInput.h new file mode 100644 index 00000000..32200f11 --- /dev/null +++ b/src/collision/narrowphase/NarrowPhaseInput.h @@ -0,0 +1,127 @@ +/******************************************************************************** +* 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_NARROW_PHASE_INPUT_H +#define REACTPHYSICS3D_NARROW_PHASE_INPUT_H + +// Libraries +#include "containers/List.h" +#include "collision/NarrowPhaseInfoBatch.h" + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Declarations +class OverlappingPair; +class CollisionShape; +struct LastFrameCollisionInfo; +struct ContactPointInfo; +class NarrowPhaseAlgorithm; +enum class NarrowPhaseAlgorithmType; +class Transform; +struct Vector3; + +// Class NarrowPhaseInput +/** + * This structure contains everything that is needed to perform the narrow-phase + * collision detection. + */ +class NarrowPhaseInput { + + private: + + NarrowPhaseInfoBatch mSphereVsSphereBatch; + NarrowPhaseInfoBatch mSphereVsCapsuleBatch; + NarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; + NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch; + NarrowPhaseInfoBatch mCapsuleVsConvexPolyhedronBatch; + NarrowPhaseInfoBatch mConvexPolyhedronVsConvexPolyhedronBatch; + + public: + + /// Constructor + NarrowPhaseInput(MemoryAllocator& allocator); + + /// Add shapes to be tested during narrow-phase collision detection into the batch + void addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, + MemoryAllocator& shapeAllocator); + + /// Get a reference to the sphere vs sphere batch + NarrowPhaseInfoBatch& getSphereVsSphereBatch(); + + /// Get a reference to the sphere vs capsule batch + NarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); + + /// Get a reference to the capsule vs capsule batch + NarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch(); + + /// Get a reference to the sphere vs convex polyhedron batch + NarrowPhaseInfoBatch& getSphereVsConvexPolyhedronBatch(); + + /// Get a reference to the capsule vs convex polyhedron batch + NarrowPhaseInfoBatch& getCapsuleVsConvexPolyhedronBatch(); + + /// Get a reference to the convex polyhedron vs convex polyhedron batch + NarrowPhaseInfoBatch& getConvexPolyhedronVsConvexPolyhedronBatch(); + + /// Clear + void clear(); +}; + + +// Get a reference to the sphere vs sphere batch +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { + return mSphereVsSphereBatch; +} + +// Get a reference to the sphere vs capsule batch +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { + return mSphereVsCapsuleBatch; +} + +// Get a reference to the capsule vs capsule batch +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() { + return mCapsuleVsCapsuleBatch; +} + +// Get a reference to the sphere vs convex polyhedron batch +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsConvexPolyhedronBatch() { + return mSphereVsConvexPolyhedronBatch; +} + +// Get a reference to the capsule vs convex polyhedron batch +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsConvexPolyhedronBatch() { + return mCapsuleVsConvexPolyhedronBatch; +} + +// Get a reference to the convex polyhedron vs convex polyhedron batch +inline NarrowPhaseInfoBatch& NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() { + return mConvexPolyhedronVsConvexPolyhedronBatch; +} + +} +#endif diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 8ef648ef..9224290a 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -52,9 +52,11 @@ SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator( } // Test collision between a sphere and a convex mesh -void SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts) const { + bool reportContacts, bool stopFirstContactFound) const { + + bool isCollisionFound = false; RP3D_PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler); @@ -133,7 +135,13 @@ void SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n } narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } } + + return isCollisionFound; } // Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction @@ -468,10 +476,12 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c } // Test collision between two convex polyhedrons -void SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts) const { +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound) const { RP3D_PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler); + bool isCollisionFound = false; + for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -537,6 +547,10 @@ void SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are still overlapping in the previous axis (the contact manifold is not empty). // Therefore, we can return without running the whole SAT algorithm narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } @@ -576,6 +590,10 @@ void SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are still overlapping in the previous axis (the contact manifold is not empty). // Therefore, we can return without running the whole SAT algorithm narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } @@ -664,6 +682,10 @@ void SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore // we return without running the whole SAT algorithm narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } @@ -855,7 +877,13 @@ void SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn } narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } } + + return isCollisionFound; } // Compute the contact points between two faces of two convex polyhedra. diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 82bd680c..1eb1b923 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -138,9 +138,9 @@ class SATAlgorithm { SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; /// Test collision between a sphere and a convex mesh - void testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, + bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts) const; + bool reportContacts, bool stopFirstContactFound) const; /// Test collision between a capsule and a convex mesh bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool reportContacts) const; @@ -157,7 +157,8 @@ class SATAlgorithm { const Vector3& edgeAdjacentFace2Normal) const; /// Test collision between two convex meshes - void testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts) const; + bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound) const; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index c2bf193a..0f98cd27 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -35,11 +35,15 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -void SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts, MemoryAllocator& memoryAllocator) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, + bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { + + bool isCollisionFound = false; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { + assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE; assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); @@ -138,7 +142,13 @@ void SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseIn } narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } } + + return isCollisionFound; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 85abc219..eab8153a 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -66,8 +66,8 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) override; }; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 11574330..b1416afd 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -35,12 +35,14 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -void SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts, MemoryAllocator& memoryAllocator) { +bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, + bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; + bool isCollisionFound = false; + #ifdef IS_PROFILING_ACTIVE gjkAlgorithm.setProfiler(mProfiler); @@ -49,13 +51,11 @@ void SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr List gjkResults(memoryAllocator); gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults, reportContacts); + assert(gjkResults.size() == batchNbItems); // For each item in the batch - uint resultIndex=0; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE || @@ -68,15 +68,19 @@ void SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr lastFrameCollisionInfo->wasUsingSAT = false; // If we have found a contact point inside the margins (shallow penetration) - if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { // Return true narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } continue; } // If we have overlap even without the margins (deep penetration) - if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) { + if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm(memoryAllocator); @@ -87,14 +91,18 @@ void SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr #endif - satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts); + isCollisionFound |= satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts, stopFirstContactFound); lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingSAT = true; + if (isCollisionFound && stopFirstContactFound) { + return isCollisionFound; + } + continue; } - - resultIndex++; } + + return isCollisionFound; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 378255e0..73251daa 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -71,8 +71,8 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) override; }; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 2c8704c7..fa042687 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -31,12 +31,15 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -void SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - bool reportContacts, MemoryAllocator& memoryAllocator) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, + bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) { + + bool isCollisionFound = false; // For each item in the batch for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { + assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE); @@ -90,6 +93,12 @@ void SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf } narrowPhaseInfoBatch.isColliding[batchIndex] = true; + isCollisionFound = true; + if (stopFirstContactFound) { + return isCollisionFound; + } } } + + return isCollisionFound; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index d5c7128f..ab68f5eb 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -65,8 +65,8 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, bool reportContacts, + virtual bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + uint batchNbItems, bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) override; }; diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 1d359a9c..3a7fb87a 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -136,8 +136,8 @@ class CollisionWorld { /// Destroy a collision body void destroyCollisionBody(CollisionBody* collisionBody); - /// Set the collision dispatch configuration - void setCollisionDispatch(CollisionDispatch* collisionDispatch); + /// Get the collision dispatch configuration + CollisionDispatch& getCollisionDispatch(); /// Ray cast method void raycast(const Ray& ray, RaycastCallback* raycastCallback, @@ -200,8 +200,8 @@ class CollisionWorld { * @param CollisionDispatch Pointer to a collision dispatch object describing * which collision detection algorithm to use for two given collision shapes */ -inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - mCollisionDetection.setCollisionDispatch(collisionDispatch); +inline CollisionDispatch& CollisionWorld::getCollisionDispatch() { + return mCollisionDetection.getCollisionDispatch(); } // Ray cast method diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 4498cbf4..99f8152a 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -56,7 +56,7 @@ OverlappingPair::~OverlappingPair() { } // Add a new last frame collision info if it does not exist for the given shapes already -void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { +LastFrameCollisionInfo* OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { // Try to get the corresponding last frame collision info const ShapeIdPair shapeIdPair(shapeId1, shapeId2); @@ -71,11 +71,15 @@ void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) // 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; } } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 2671edfd..d25d20af 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -180,7 +180,7 @@ class OverlappingPair { void reduceContactManifolds(); /// Add a new last frame collision info if it does not exist for the given shapes already - void addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2); + 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;