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

Refactor narrow-phase algorithms to iterate over batches of narrow-phase infos

Browse Source
This commit is contained in:
Daniel Chappuis 2018-10-03 22:10:05 +02:00
parent cf3d76ce45
commit e8ed10314a
30 changed files with 1279 additions and 1189 deletions

4
CMakeLists.txt
View File

@ -115,7 +115,7 @@ SET (REACTPHYSICS3D_HEADERS
"src/collision/PolyhedronMesh.h" "src/collision/PolyhedronMesh.h"
"src/collision/HalfEdgeStructure.h" "src/collision/HalfEdgeStructure.h"
"src/collision/CollisionDetection.h" "src/collision/CollisionDetection.h"
"src/collision/NarrowPhaseInfo.h" "src/collision/NarrowPhaseInfoBatch.h"
"src/collision/ContactManifold.h" "src/collision/ContactManifold.h"
"src/collision/ContactManifoldSet.h" "src/collision/ContactManifoldSet.h"
"src/collision/MiddlePhaseTriangleCallback.h" "src/collision/MiddlePhaseTriangleCallback.h"
@ -198,7 +198,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/PolyhedronMesh.cpp" "src/collision/PolyhedronMesh.cpp"
"src/collision/HalfEdgeStructure.cpp" "src/collision/HalfEdgeStructure.cpp"
"src/collision/CollisionDetection.cpp" "src/collision/CollisionDetection.cpp"
"src/collision/NarrowPhaseInfo.cpp" "src/collision/NarrowPhaseInfoBatch.cpp"
"src/collision/ContactManifold.cpp" "src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.cpp" "src/collision/ContactManifoldSet.cpp"
"src/collision/MiddlePhaseTriangleCallback.cpp" "src/collision/MiddlePhaseTriangleCallback.cpp"

227
src/collision/CollisionDetection.cpp
View File

@ -35,7 +35,7 @@
#include "collision/CollisionCallback.h" #include "collision/CollisionCallback.h"
#include "collision/MiddlePhaseTriangleCallback.h" #include "collision/MiddlePhaseTriangleCallback.h"
#include "collision/OverlapCallback.h" #include "collision/OverlapCallback.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
#include "collision/ContactManifold.h" #include "collision/ContactManifold.h"
#include "utils/Profiler.h" #include "utils/Profiler.h"
#include "engine/EventListener.h" #include "engine/EventListener.h"
@ -49,7 +49,7 @@ using namespace std;
// Constructor // Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager)
: mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfos(mMemoryManager.getPoolAllocator()), : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoBatch(mMemoryManager.getPoolAllocator()),
mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this), mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this),
mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) { mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) {
@ -159,17 +159,15 @@ void CollisionDetection::computeMiddlePhase() {
// No middle-phase is necessary, simply create a narrow phase info // No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection // for the narrow-phase collision detection
NarrowPhaseInfo* narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(NarrowPhaseInfo))) mNarrowPhaseInfoBatch.addNarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(),
NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), mMemoryManager.getSingleFrameAllocator());
shape2->getLocalToWorldTransform(), mMemoryManager.getSingleFrameAllocator());
mNarrowPhaseInfos.add(narrowPhaseInfo);
} }
// Concave vs Convex algorithm // Concave vs Convex algorithm
else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) {
computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), mNarrowPhaseInfos); computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), mNarrowPhaseInfoBatch);
} }
// Concave vs Concave shape // Concave vs Concave shape
else { else {
@ -185,7 +183,7 @@ void CollisionDetection::computeMiddlePhase() {
// Compute the concave vs convex middle-phase algorithm for a given pair of bodies // Compute the concave vs convex middle-phase algorithm for a given pair of bodies
void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
List<NarrowPhaseInfo*>& narrowPhaseInfos) { NarrowPhaseInfoBatch& narrowPhaseInfoBatch) {
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair->getShape2();
@ -211,7 +209,7 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair
// Set the parameters of the callback object // Set the parameters of the callback object
MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape,
concaveShape, narrowPhaseInfos, allocator); concaveShape, narrowPhaseInfoBatch, allocator);
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE
@ -235,33 +233,32 @@ void CollisionDetection::computeNarrowPhase() {
RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler);
List<NarrowPhaseInfo*> collidingNarrowPhaseInfos(mMemoryManager.getSingleFrameAllocator()); List<uint> collidingBatchIndices(mMemoryManager.getSingleFrameAllocator());
// For each narrow phase info to process // For each narrow phase info to process
for(uint i=0; i < mNarrowPhaseInfos.size(); i++) { for(uint batchIndex=0; batchIndex < mNarrowPhaseInfoBatch.getNbObjects(); batchIndex++) {
NarrowPhaseInfo* narrowPhaseInfo = mNarrowPhaseInfos[i]; assert(mNarrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
assert(narrowPhaseInfo->contactPoints.size() == 0);
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape1Type = mNarrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType();
const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); const CollisionShapeType shape2Type = mNarrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is no collision algorithm between those two kinds of shapes, skip it // If there is no collision algorithm between those two kinds of shapes, skip it
if (narrowPhaseAlgorithm != nullptr) { if (narrowPhaseAlgorithm != nullptr) {
LastFrameCollisionInfo* lastCollisionFrameInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); LastFrameCollisionInfo* lastCollisionFrameInfo = mNarrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) { narrowPhaseAlgorithm->testCollision(mNarrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getSingleFrameAllocator());
if (mNarrowPhaseInfoBatch.isColliding[batchIndex]) {
lastCollisionFrameInfo->wasColliding = true; lastCollisionFrameInfo->wasColliding = true;
collidingNarrowPhaseInfos.add(narrowPhaseInfo); collidingBatchIndices.add(batchIndex);
} }
else { else {
lastCollisionFrameInfo->wasColliding = false; lastCollisionFrameInfo->wasColliding = false;
@ -273,7 +270,7 @@ void CollisionDetection::computeNarrowPhase() {
} }
// Convert the potential contact into actual contacts // Convert the potential contact into actual contacts
processAllPotentialContacts(collidingNarrowPhaseInfos, mOverlappingPairs); processAllPotentialContacts(mNarrowPhaseInfoBatch, collidingBatchIndices, mOverlappingPairs);
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
@ -281,20 +278,8 @@ void CollisionDetection::computeNarrowPhase() {
// Report contacts to the user // Report contacts to the user
reportAllContacts(); reportAllContacts();
// Destroy the narrow phase infos
for(uint i=0; i < mNarrowPhaseInfos.size(); i++) {
NarrowPhaseInfo* narrowPhaseInfo = mNarrowPhaseInfos[i];
// Call the destructor
narrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory for the narrow phase info
mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Clear the list of narrow-phase infos // Clear the list of narrow-phase infos
mNarrowPhaseInfos.clear(); mNarrowPhaseInfoBatch.clear();
} }
// Allow the broadphase to notify the collision detection about an overlapping pair. // Allow the broadphase to notify the collision detection about an overlapping pair.
@ -423,24 +408,23 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
} }
/// Convert the potential contact into actual contacts /// Convert the potential contact into actual contacts
void CollisionDetection::processAllPotentialContacts(const List<NarrowPhaseInfo*>& collidingNarrowPhaseInfos, void CollisionDetection::processAllPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, const List<uint>& collidingBatchIndex,
const OverlappingPairMap& overlappingPairs) { const OverlappingPairMap& overlappingPairs) {
RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler);
// For each narrow phase info object // For each narrow phase info object
for(uint i=0; i < collidingNarrowPhaseInfos.size(); i++) { for(uint i=0; i < collidingBatchIndex.size(); i++) {
NarrowPhaseInfo* narrowPhaseInfo = collidingNarrowPhaseInfos[i]; uint batchIndex = collidingBatchIndex[i];
assert(narrowPhaseInfo != nullptr); assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
assert(narrowPhaseInfo->contactPoints.size() > 0);
// Transfer the contact points from the narrow phase info to the overlapping pair // Transfer the contact points from the narrow phase info to the overlapping pair
narrowPhaseInfo->overlappingPair->addPotentialContactPoints(narrowPhaseInfo); narrowPhaseInfoBatch.overlappingPairs[batchIndex]->addPotentialContactPoints(narrowPhaseInfoBatch, batchIndex);
// Remove the contacts points from the narrow phase info object. // Remove the contacts points from the narrow phase info object.
narrowPhaseInfo->resetContactPoints(); narrowPhaseInfoBatch.resetContactPoints(batchIndex);
} }
// For each overlapping pairs in contact during the narrow-phase // For each overlapping pairs in contact during the narrow-phase
@ -478,7 +462,7 @@ void CollisionDetection::reportAllContacts() {
} }
// Compute the middle-phase collision detection between two proxy shapes // Compute the middle-phase collision detection between two proxy shapes
void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, List<NarrowPhaseInfo*>& outNarrowPhaseInfos) { void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch) {
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair->getShape2();
@ -495,11 +479,9 @@ void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair,
// No middle-phase is necessary, simply create a narrow phase info // No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection // for the narrow-phase collision detection
NarrowPhaseInfo* narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, outNarrowPhaseInfoBatch.addNarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(),
sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator());
shape2->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator());
outNarrowPhaseInfos.add(narrowPhaseInfo);
} }
// Concave vs Convex algorithm // Concave vs Convex algorithm
@ -507,7 +489,7 @@ void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair,
// Run the middle-phase collision detection algorithm to find the triangles of the concave // 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 // shape we need to use during the narrow-phase collision detection
computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), outNarrowPhaseInfos); computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), outNarrowPhaseInfoBatch);
} }
pair->clearObsoleteLastFrameCollisionInfos(); pair->clearObsoleteLastFrameCollisionInfos();
@ -556,7 +538,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
// Return true if two bodies overlap // Return true if two bodies overlap
bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) { bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) {
List<NarrowPhaseInfo*> narrowPhaseInfos(mMemoryManager.getPoolAllocator()); NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator());
// For each proxy shape proxy shape of the first body // For each proxy shape proxy shape of the first body
ProxyShape* body1ProxyShape = body1->getProxyShapesList(); ProxyShape* body1ProxyShape = body1->getProxyShapesList();
@ -577,23 +559,19 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(),
mMemoryManager.getPoolAllocator(), mWorld->mConfig); mMemoryManager.getPoolAllocator(), mWorld->mConfig);
narrowPhaseInfos.clear();
// Compute the middle-phase collision detection between the two shapes // Compute the middle-phase collision detection between the two shapes
computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInfos); computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInfoBatch);
bool isColliding = false; bool isColliding = false;
// For each narrow-phase info object // For each narrow-phase info object
for(uint i=0; i < narrowPhaseInfos.size(); i++) { for(uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) {
NarrowPhaseInfo* narrowPhaseInfo = narrowPhaseInfos[i];
// If we have not found a collision yet // If we have not found a collision yet
if (!isColliding) { if (!isColliding) {
const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType();
const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType();
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@ -604,16 +582,13 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, false, mMemoryManager.getPoolAllocator());
isColliding |= narrowPhaseInfoBatch.isColliding[batchIndex];
}
} }
} }
// Call the destructor narrowPhaseInfoBatch.clear();
narrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision // Return if we have found a narrow-phase collision
if (isColliding) return true; if (isColliding) return true;
@ -638,7 +613,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
assert(overlapCallback != nullptr); assert(overlapCallback != nullptr);
Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator()); Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator());
List<NarrowPhaseInfo*> narrowPhaseInfos(mMemoryManager.getPoolAllocator()); NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator());
// For each proxy shape proxy shape of the body // For each proxy shape proxy shape of the body
ProxyShape* bodyProxyShape = body->getProxyShapesList(); ProxyShape* bodyProxyShape = body->getProxyShapesList();
@ -675,23 +650,19 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(),
mMemoryManager.getPoolAllocator(), mWorld->mConfig); mMemoryManager.getPoolAllocator(), mWorld->mConfig);
narrowPhaseInfos.clear();
// Compute the middle-phase collision detection between the two shapes // Compute the middle-phase collision detection between the two shapes
computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInfos); computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInfoBatch);
bool isColliding = false; bool isColliding = false;
// For each narrow-phase info object // For each narrow-phase info object
for (uint i=0; i<narrowPhaseInfos.size(); i++) { for (uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) {
NarrowPhaseInfo* narrowPhaseInfo = narrowPhaseInfos[i];
// If we have not found a collision yet // If we have not found a collision yet
if (!isColliding) { if (!isColliding) {
const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType();
const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType();
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@ -702,16 +673,13 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, false, mMemoryManager.getPoolAllocator());
isColliding |= narrowPhaseInfoBatch.isColliding[batchIndex];
}
} }
} }
// Call the destructor narrowPhaseInfoBatch.clear();
narrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision // Return if we have found a narrow-phase collision
if (isColliding) { if (isColliding) {
@ -742,8 +710,8 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
assert(collisionCallback != nullptr); assert(collisionCallback != nullptr);
List<NarrowPhaseInfo*> collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); List<uint> collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator());
List<NarrowPhaseInfo*> allNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator());
OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
// For each proxy shape proxy shape of the first body // For each proxy shape proxy shape of the first body
@ -784,7 +752,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
} }
// Compute the middle-phase collision detection between the two shapes // Compute the middle-phase collision detection between the two shapes
computeMiddlePhaseForProxyShapes(pair, allNarrowPhaseInfos); computeMiddlePhaseForProxyShapes(pair, narrowPhaseInfoBatch);
} }
// Go to the next proxy shape // Go to the next proxy shape
@ -796,12 +764,10 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
} }
// For each narrow-phase info object // For each narrow-phase info object
for (uint i=0; i < allNarrowPhaseInfos.size(); i++) { for (uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) {
NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos[i]; const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType();
const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType();
const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@ -812,15 +778,16 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getPoolAllocator());
if (narrowPhaseInfoBatch.isColliding[batchIndex]) {
collidingNarrowPhaseInfos.add(narrowPhaseInfo); collidingNarrowPhaseInfos.add(batchIndex);
} }
} }
} }
// Process the potential contacts // Process the potential contacts
processAllPotentialContacts(collidingNarrowPhaseInfos, overlappingPairs); processAllPotentialContacts(narrowPhaseInfoBatch, collidingNarrowPhaseInfos, overlappingPairs);
// For each overlapping pair // For each overlapping pair
for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
@ -838,18 +805,6 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
pair->~OverlappingPair(); pair->~OverlappingPair();
mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
} }
// Destroy the narrow phase infos
for (uint i=0; i < allNarrowPhaseInfos.size(); i++) {
NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos[i];
// Call the destructor
narrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory for the narrow phase info
mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
} }
// Test and report collisions between a body and all the others bodies of the world // Test and report collisions between a body and all the others bodies of the world
@ -857,8 +812,8 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
assert(callback != nullptr); assert(callback != nullptr);
List<NarrowPhaseInfo*> collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); List<uint> collidingBatchIndices(mMemoryManager.getPoolAllocator());
List<NarrowPhaseInfo*> allNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator());
OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
// For each proxy shape proxy shape of the body // For each proxy shape proxy shape of the body
@ -913,7 +868,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
} }
// Compute the middle-phase collision detection between the two shapes // Compute the middle-phase collision detection between the two shapes
computeMiddlePhaseForProxyShapes(pair, allNarrowPhaseInfos); computeMiddlePhaseForProxyShapes(pair, narrowPhaseInfoBatch);
} }
} }
@ -927,12 +882,10 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
} }
// For each narrow-phase info object // For each narrow-phase info object
for (auto it = allNarrowPhaseInfos.begin(); it != allNarrowPhaseInfos.end(); ++it) { for (uint batchIndex = 0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) {
NarrowPhaseInfo* narrowPhaseInfo = *it; const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType();
const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType();
const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@ -943,15 +896,16 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getPoolAllocator());
if (narrowPhaseInfoBatch.isColliding[batchIndex]) {
collidingNarrowPhaseInfos.add(narrowPhaseInfo); collidingBatchIndices.add(batchIndex);
} }
} }
} }
// Process the potential contacts // Process the potential contacts
processAllPotentialContacts(collidingNarrowPhaseInfos, overlappingPairs); processAllPotentialContacts(narrowPhaseInfoBatch, collidingBatchIndices, overlappingPairs);
// For each overlapping pair // For each overlapping pair
for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
@ -969,18 +923,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
pair->~OverlappingPair(); pair->~OverlappingPair();
mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
} }
// Destroy the narrow phase infos
for (uint i=0; i < allNarrowPhaseInfos.size(); i++) {
NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos[i];
// Call the destructor
narrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory for the narrow phase info
mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
} }
// Test and report collisions between all shapes of the world // Test and report collisions between all shapes of the world
@ -991,8 +933,8 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
List<NarrowPhaseInfo*> collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); List<uint> collidingBatchIndices(mMemoryManager.getPoolAllocator());
List<NarrowPhaseInfo*> allNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); NarrowPhaseInfoBatch narrowPhaseInfoBatch(mMemoryManager.getPoolAllocator());
OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
// For each possible collision pair of bodies // For each possible collision pair of bodies
@ -1032,17 +974,15 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
// Compute the middle-phase collision detection between the two shapes // Compute the middle-phase collision detection between the two shapes
computeMiddlePhaseForProxyShapes(pair, allNarrowPhaseInfos); computeMiddlePhaseForProxyShapes(pair, narrowPhaseInfoBatch);
} }
} }
// For each narrow-phase info object // For each narrow-phase info object
for (uint i=0; i < allNarrowPhaseInfos.size(); i++) { for (uint batchIndex=0; batchIndex < narrowPhaseInfoBatch.getNbObjects(); batchIndex++) {
NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos[i]; const CollisionShapeType shape1Type = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType();
const CollisionShapeType shape2Type = narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType();
const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@ -1053,15 +993,16 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { narrowPhaseAlgorithm->testCollision(narrowPhaseInfoBatch, batchIndex, 1, true, mMemoryManager.getPoolAllocator());
if (narrowPhaseInfoBatch.isColliding[batchIndex]) {
collidingNarrowPhaseInfos.add(narrowPhaseInfo); collidingBatchIndices.add(batchIndex);
} }
} }
} }
// Process the potential contacts // Process the potential contacts
processAllPotentialContacts(collidingNarrowPhaseInfos, overlappingPairs); processAllPotentialContacts(narrowPhaseInfoBatch, collidingBatchIndices, overlappingPairs);
// For each overlapping pair // For each overlapping pair
for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
@ -1079,18 +1020,6 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
pair->~OverlappingPair(); pair->~OverlappingPair();
mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
} }
// Destroy the narrow phase infos
for (uint i=0; i < allNarrowPhaseInfos.size(); i++) {
NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos[i];
// Call the destructor
narrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory for the narrow phase info
mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
} }
// Fill-in the collision detection matrix // Fill-in the collision detection matrix

10
src/collision/CollisionDetection.h
View File

@ -32,6 +32,7 @@
#include "collision/shapes/CollisionShape.h" #include "collision/shapes/CollisionShape.h"
#include "engine/OverlappingPair.h" #include "engine/OverlappingPair.h"
#include "collision/narrowphase/DefaultCollisionDispatch.h" #include "collision/narrowphase/DefaultCollisionDispatch.h"
#include "collision/NarrowPhaseInfoBatch.h"
#include "containers/Map.h" #include "containers/Map.h"
#include "containers/Set.h" #include "containers/Set.h"
@ -79,7 +80,7 @@ class CollisionDetection {
CollisionWorld* mWorld; CollisionWorld* mWorld;
/// List of narrow phase infos /// List of narrow phase infos
List<NarrowPhaseInfo*> mNarrowPhaseInfos; NarrowPhaseInfoBatch mNarrowPhaseInfoBatch;
/// Broad-phase overlapping pairs /// Broad-phase overlapping pairs
OverlappingPairMap mOverlappingPairs; OverlappingPairMap mOverlappingPairs;
@ -127,13 +128,14 @@ class CollisionDetection {
/// Compute the concave vs convex middle-phase algorithm for a given pair of bodies /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
List<NarrowPhaseInfo*>& narrowPhaseInfos); NarrowPhaseInfoBatch& narrowPhaseInfoBatch);
/// Compute the middle-phase collision detection between two proxy shapes /// Compute the middle-phase collision detection between two proxy shapes
void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, List<NarrowPhaseInfo*>& outNarrowPhaseInfos); void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch);
/// Convert the potential contact into actual contacts /// Convert the potential contact into actual contacts
void processAllPotentialContacts(const List<NarrowPhaseInfo*>& collidingNarrowPhaseInfos, void processAllPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
const List<uint>& collidingBatchIndex,
const OverlappingPairMap& overlappingPairs); const OverlappingPairMap& overlappingPairs);
/// Report contacts for all the colliding overlapping pairs /// Report contacts for all the colliding overlapping pairs

10
src/collision/ContactManifoldSet.cpp
View File

@ -25,7 +25,7 @@
// Libraries // Libraries
#include "ContactManifoldSet.h" #include "ContactManifoldSet.h"
#include "NarrowPhaseInfo.h" #include "NarrowPhaseInfoBatch.h"
#include "constraint/ContactPoint.h" #include "constraint/ContactPoint.h"
#include "ProxyShape.h" #include "ProxyShape.h"
#include "collision/ContactManifold.h" #include "collision/ContactManifold.h"
@ -49,14 +49,14 @@ ContactManifoldSet::~ContactManifoldSet() {
clear(); clear();
} }
void ContactManifoldSet::addContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { void ContactManifoldSet::addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
assert(narrowPhaseInfo->contactPoints.size() > 0); assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
// For each potential contact point to add // For each potential contact point to add
for (uint i=0; i < narrowPhaseInfo->contactPoints.size(); i++) { for (uint i=0; i < narrowPhaseInfoBatch.contactPoints[batchIndex].size(); i++) {
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints[i]; ContactPointInfo* contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][i];
// Look if the contact point correspond to an existing potential manifold // Look if the contact point correspond to an existing potential manifold
// (if the contact point normal is similar to the normal of an existing manifold) // (if the contact point normal is similar to the normal of an existing manifold)

7
src/collision/ContactManifoldSet.h
View File

@ -26,6 +26,9 @@
#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H #ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H #define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
// Libraries
#include "configuration.h"
namespace reactphysics3d { namespace reactphysics3d {
// Declarations // Declarations
@ -34,7 +37,7 @@ class ContactManifoldInfo;
class ProxyShape; class ProxyShape;
class MemoryAllocator; class MemoryAllocator;
struct WorldSettings; struct WorldSettings;
struct NarrowPhaseInfo; struct NarrowPhaseInfoBatch;
struct Vector3; struct Vector3;
class CollisionShape; class CollisionShape;
class Transform; class Transform;
@ -110,7 +113,7 @@ class ContactManifoldSet {
~ContactManifoldSet(); ~ContactManifoldSet();
/// Add the contact points from the narrow phase /// Add the contact points from the narrow phase
void addContactPoints(NarrowPhaseInfo* narrowPhaseInfo); void addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
/// Return the first proxy shape /// Return the first proxy shape
ProxyShape* getShape1() const; ProxyShape* getShape1() const;

10
src/collision/MiddlePhaseTriangleCallback.cpp
View File

@ -26,8 +26,8 @@
// Libraries // Libraries
#include "collision/MiddlePhaseTriangleCallback.h" #include "collision/MiddlePhaseTriangleCallback.h"
#include "engine/OverlappingPair.h" #include "engine/OverlappingPair.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/TriangleShape.h" #include "collision/shapes/TriangleShape.h"
#include "collision/NarrowPhaseInfoBatch.h"
using namespace reactphysics3d; using namespace reactphysics3d;
@ -51,12 +51,8 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co
ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape; ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape;
// Create a narrow phase info for the narrow-phase collision detection // Create a narrow phase info for the narrow-phase collision detection
NarrowPhaseInfo* narrowPhaseInfo = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) mOutNarrowPhaseInfoBatch.addNarrowPhaseInfo(mOverlappingPair,
NarrowPhaseInfo(mOverlappingPair,
isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape, isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape,
isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(), isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
shape1->getLocalToWorldTransform(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), mAllocator);
shape2->getLocalToWorldTransform(),
mAllocator);
mOutNarrowPhaseInfos.add(narrowPhaseInfo);
} }

10
src/collision/MiddlePhaseTriangleCallback.h
View File

@ -41,7 +41,7 @@ class NarrowPhaseAlgorithm;
class ProxyShape; class ProxyShape;
class MemoryAllocator; class MemoryAllocator;
class Profiler; class Profiler;
struct NarrowPhaseInfo; struct NarrowPhaseInfoBatch;
struct Vector3; struct Vector3;
// Class ConvexVsTriangleCallback // Class ConvexVsTriangleCallback
@ -66,8 +66,8 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
/// Pointer to the concave collision shape /// Pointer to the concave collision shape
const ConcaveShape* mConcaveShape; const ConcaveShape* mConcaveShape;
/// Reference to the list of narrow-phase infos /// Reference to the narrow phase info batch
List<NarrowPhaseInfo*>& mOutNarrowPhaseInfos; NarrowPhaseInfoBatch& mOutNarrowPhaseInfoBatch;
/// Reference to the single-frame memory allocator /// Reference to the single-frame memory allocator
MemoryAllocator& mAllocator; MemoryAllocator& mAllocator;
@ -85,11 +85,11 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
ProxyShape* concaveProxyShape, ProxyShape* concaveProxyShape,
ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
List<NarrowPhaseInfo*>& outNarrowPhaseInfos, NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch,
MemoryAllocator& allocator) MemoryAllocator& allocator)
:mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
mOutNarrowPhaseInfos(outNarrowPhaseInfos), mAllocator(allocator) { mOutNarrowPhaseInfoBatch(outNarrowPhaseInfoBatch), mAllocator(allocator) {
} }

89
src/collision/NarrowPhaseInfo.cpp → src/collision/NarrowPhaseInfoBatch.cpp
View File

@ -24,7 +24,7 @@
********************************************************************************/ ********************************************************************************/
// Libraries // Libraries
#include "NarrowPhaseInfo.h" #include "NarrowPhaseInfoBatch.h"
#include "ContactPointInfo.h" #include "ContactPointInfo.h"
#include "collision/shapes/TriangleShape.h" #include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h" #include "engine/OverlappingPair.h"
@ -32,61 +32,63 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator)
CollisionShape* shape2, const Transform& shape1Transform, : mMemoryAllocator(allocator), overlappingPairs(allocator), collisionShapes1(allocator), collisionShapes2(allocator),
const Transform& shape2Transform, MemoryAllocator& shapeAllocator) shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator),
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator) {
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(overlappingPair->getTemporaryAllocator()), collisionShapeAllocator(shapeAllocator) {
// Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
} }
// Destructor // Destructor
NarrowPhaseInfo::~NarrowPhaseInfo() { NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() {
clear();
assert(contactPoints.size() == 0);
// Release the memory of the TriangleShape (this memory was allocated in the
// MiddlePhaseTriangleCallback::testTriangle() method)
if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
collisionShape1->~CollisionShape();
collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape));
}
if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
collisionShape2->~CollisionShape();
collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
} }
// Add shapes to be tested during narrow-phase collision detection into the batch
void NarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
const Transform& shape1Transform, const Transform& shape2Transform,
MemoryAllocator& shapeAllocator) {
overlappingPairs.add(pair);
collisionShapes1.add(shape1);
collisionShapes2.add(shape2);
shape1ToWorldTransforms.add(shape1Transform);
shape2ToWorldTransforms.add(shape2Transform);
collisionShapeAllocators.add(&shapeAllocator);
contactPoints.add(List<ContactPointInfo*>(mMemoryAllocator));
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());
} }
// Add a new contact point // Add a new contact point
void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) { const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0)); assert(penDepth > decimal(0.0));
// Get the memory allocator // Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator(); MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator();
// Create the contact point info // Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the list of contact points // Add it into the list of contact points
contactPoints.add(contactPointInfo); contactPoints[index].add(contactPointInfo);
} }
// Reset the remaining contact points // Reset the remaining contact points
void NarrowPhaseInfo::resetContactPoints() { void NarrowPhaseInfoBatch::resetContactPoints(uint index) {
// Get the memory allocator // Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator(); MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator();
// For each remaining contact point info // For each remaining contact point info
for (uint i=0; i < contactPoints.size(); i++) { for (uint i=0; i < contactPoints[index].size(); i++) {
ContactPointInfo* contactPoint = contactPoints[i]; ContactPointInfo* contactPoint = contactPoints[index][i];
// Call the destructor // Call the destructor
contactPoint->~ContactPointInfo(); contactPoint->~ContactPointInfo();
@ -95,5 +97,34 @@ void NarrowPhaseInfo::resetContactPoints() {
allocator.release(contactPoint, sizeof(ContactPointInfo)); allocator.release(contactPoint, sizeof(ContactPointInfo));
} }
contactPoints[index].clear();
}
// Clear all the objects in the batch
void NarrowPhaseInfoBatch::clear() {
for (uint i=0; i < overlappingPairs.size(); i++) {
assert(contactPoints[i].size() == 0);
// Release the memory of the TriangleShape (this memory was allocated in the
// MiddlePhaseTriangleCallback::testTriangle() method)
if (collisionShapes1[i]->getName() == CollisionShapeName::TRIANGLE) {
collisionShapes1[i]->~CollisionShape();
collisionShapeAllocators[i]->release(collisionShapes1[i], sizeof(TriangleShape));
}
if (collisionShapes2[i]->getName() == CollisionShapeName::TRIANGLE) {
collisionShapes2[i]->~CollisionShape();
collisionShapeAllocators[i]->release(collisionShapes2[i], sizeof(TriangleShape));
}
}
overlappingPairs.clear();
collisionShapes1.clear();
collisionShapes2.clear();
shape1ToWorldTransforms.clear();
shape2ToWorldTransforms.clear();
collisionShapeAllocators.clear();
isColliding.clear();
contactPoints.clear(); contactPoints.clear();
} }

80
src/collision/NarrowPhaseInfo.h → src/collision/NarrowPhaseInfoBatch.h
View File

@ -23,8 +23,8 @@
* * * *
********************************************************************************/ ********************************************************************************/
#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_H #ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_BATCH_H
#define REACTPHYSICS3D_NARROW_PHASE_INFO_H #define REACTPHYSICS3D_NARROW_PHASE_INFO_BATCH_H
// Libraries // Libraries
#include "engine/OverlappingPair.h" #include "engine/OverlappingPair.h"
@ -38,58 +38,82 @@ struct LastFrameCollisionInfo;
class ContactManifoldInfo; class ContactManifoldInfo;
struct ContactPointInfo; struct ContactPointInfo;
// Class NarrowPhaseInfo // Struct NarrowPhaseInfoBatch
/** /**
* This structure regroups different things about a collision shape. This is * This abstract structure collects all the potential collisions from the middle-phase algorithm
* used to pass information about a collision shape to a collision algorithm. * that have to be tested during narrow-phase collision detection. There is an implementation of
* this class for each kind of collision detection test. For instance, one for sphere vs sphere,
* one for sphere vs capsule, ...
*/ */
struct NarrowPhaseInfo { struct NarrowPhaseInfoBatch {
private:
/// Memory allocator
MemoryAllocator& mMemoryAllocator;
public: public:
/// Broadphase overlapping pair /// List of Broadphase overlapping pairs
OverlappingPair* overlappingPair; List<OverlappingPair*> overlappingPairs;
/// Pointer to the first collision shape to test collision with /// List of pointers to the first collision shapes to test collision with
CollisionShape* collisionShape1; List<CollisionShape*> collisionShapes1;
/// Pointer to the second collision shape to test collision with /// List of pointers to the second collision shapes to test collision with
CollisionShape* collisionShape2; List<CollisionShape*> collisionShapes2;
/// Transform that maps from collision shape 1 local-space to world-space /// List of transforms that maps from collision shape 1 local-space to world-space
Transform shape1ToWorldTransform; List<Transform> shape1ToWorldTransforms;
/// Transform that maps from collision shape 2 local-space to world-space /// List of transforms that maps from collision shape 2 local-space to world-space
Transform shape2ToWorldTransform; List<Transform> shape2ToWorldTransforms;
/// Result of the narrow-phase collision detection test
List<bool> isColliding;
/// List of contact points created during the narrow-phase /// List of contact points created during the narrow-phase
List<ContactPointInfo*> contactPoints; List<List<ContactPointInfo*>> contactPoints;
/// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) /// Memory allocators for the collision shape (Used to release TriangleShape memory in destructor)
MemoryAllocator& collisionShapeAllocator; List<MemoryAllocator*> collisionShapeAllocators;
/// Constructor /// Constructor
NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, NarrowPhaseInfoBatch(MemoryAllocator& allocator);
/// Destructor
~NarrowPhaseInfoBatch();
/// Return the number of objects in the batch
uint getNbObjects() const;
/// Add shapes to be tested during narrow-phase collision detection into the batch
void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform, CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator); const Transform& shape2Transform, MemoryAllocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
/// Add a new contact point /// Add a new contact point
void addContactPoint(const Vector3& contactNormal, decimal penDepth, void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2); const Vector3& localPt1, const Vector3& localPt2);
/// Reset the remaining contact points /// Reset the remaining contact points
void resetContactPoints(); void resetContactPoints(uint index);
/// Clear all the objects in the batch
void clear();
/// Get the last collision frame info for temporal coherence /// Get the last collision frame info for temporal coherence
LastFrameCollisionInfo* getLastFrameCollisionInfo() const; LastFrameCollisionInfo* getLastFrameCollisionInfo(uint index) const;
}; };
/// Return the number of objects in the batch
inline uint NarrowPhaseInfoBatch::getNbObjects() const {
return overlappingPairs.size();
}
// Get the last collision frame info for temporal coherence // Get the last collision frame info for temporal coherence
inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const { inline LastFrameCollisionInfo* NarrowPhaseInfoBatch::getLastFrameCollisionInfo(uint index) const {
return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId()); return overlappingPairs[index]->getLastFrameCollisionInfo(collisionShapes1[index]->getId(), collisionShapes2[index]->getId());
} }
} }

46
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
View File

@ -26,7 +26,7 @@
// Libraries // Libraries
#include "CapsuleVsCapsuleAlgorithm.h" #include "CapsuleVsCapsuleAlgorithm.h"
#include "collision/shapes/CapsuleShape.h" #include "collision/shapes/CapsuleShape.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -34,18 +34,22 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two capsules // Compute the narrow-phase collision detection between two capsules
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius. // by Dirk Gregorius.
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, void CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) { MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// Get the capsule collision shapes // Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1); const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2); const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// Get the transform from capsule 1 local-space to capsule 2 local-space // Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() *
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
// Compute the end-points of the inner segment of the first capsule // Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0); Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
@ -74,7 +78,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false; continue;
} }
// Compute the planes that goes through the extreme points of the inner segment of capsule 1 // Compute the planes that goes through the extreme points of the inner segment of capsule 1
@ -139,14 +143,15 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance; decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized; const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
} }
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
} }
} }
@ -174,12 +179,12 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2;
decimal penetrationDepth = sumRadius - closestPointsDistance; decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
} }
else { // The segment are overlapping (degenerate case) else { // The segment are overlapping (degenerate case)
@ -197,10 +202,10 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius()); const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
} }
else { // If the capsules inner segments are not parallel else { // If the capsules inner segments are not parallel
@ -213,16 +218,15 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius()); const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
} }
} }
} }
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
} }
return false;
} }

6
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
View File

@ -28,11 +28,13 @@
// Libraries // Libraries
#include "NarrowPhaseAlgorithm.h" #include "NarrowPhaseAlgorithm.h"
#include "configuration.h"
/// Namespace ReactPhysics3D /// Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
// Declarations // Declarations
struct NarrowPhaseInfoBatch;
class Body; class Body;
class ContactPoint; class ContactPoint;
@ -65,7 +67,9 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules /// Compute the narrow-phase collision detection between two capsules
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) override;
}; };
} }

70
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
View File

@ -29,7 +29,7 @@
#include "GJK/GJKAlgorithm.h" #include "GJK/GJKAlgorithm.h"
#include "collision/shapes/CapsuleShape.h" #include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexPolyhedronShape.h" #include "collision/shapes/ConvexPolyhedronShape.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
#include "collision/ContactPointInfo.h" #include "collision/ContactPointInfo.h"
#include <cassert> #include <cassert>
@ -39,7 +39,9 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron // 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 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius. // by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, void CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool reportContacts,
MemoryAllocator& memoryAllocator) { MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm // First, we run the GJK algorithm
@ -53,43 +55,51 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
#endif #endif
// Get the last frame collision info // Run the GJK algorithm
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); List<GJKAlgorithm::GJKResult> gjkResults(memoryAllocator);
gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults, reportContacts);
assert(gjkResults.size() == batchNbItems);
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); uint resultIndex = 0;
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
lastFrameCollisionInfo->wasUsingGJK = true; lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false; lastFrameCollisionInfo->wasUsingSAT = false;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// If we have found a contact point inside the margins (shallow penetration) // If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
if (reportContacts) { if (reportContacts) {
bool noContact = false;
// GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
// capsule inner segment and parallel to the contact point normal, we would like to create // capsule inner segment and parallel to the contact point normal, we would like to create
// two contact points instead of a single one (as in the deep contact case with SAT algorithm) // two contact points instead of a single one (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK // Get the contact point created by GJK
assert(narrowPhaseInfo->contactPoints.size() > 0); assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
ContactPointInfo*& contactPoint = narrowPhaseInfo->contactPoints[0]; ContactPointInfo*& contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][0];
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE;
// Get the collision shapes // Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
// For each face of the polyhedron // For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) { for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// Get the face normal // Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f); const Vector3 faceNormal = polyhedron->getFaceNormal(f);
@ -109,9 +119,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) { && areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK // Remove the previous contact point computed by GJK
narrowPhaseInfo->resetContactPoints(); narrowPhaseInfoBatch.resetContactPoints(batchIndex);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
// Compute the end-points of the inner segment of the capsule // Compute the end-points of the inner segment of the capsule
@ -133,34 +143,40 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1); narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
if (!contactsFound) { if (!contactsFound) {
return false; noContact = true;
narrowPhaseInfoBatch.isColliding[batchIndex] = false;
break;
} }
break; break;
} }
} }
if (noContact) {
continue;
}
} }
lastFrameCollisionInfo->wasUsingSAT = false; lastFrameCollisionInfo->wasUsingSAT = false;
lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingGJK = false;
// Return true // Return true
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
} }
// If we have overlap even without the margins (deep penetration) // If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point // Run the SAT algorithm to find the separating axis and compute contact point
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true; lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
} }
return false; resultIndex++;
}
} }

4
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
View File

@ -70,7 +70,9 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a capsule and a polyhedron /// Compute the narrow-phase collision detection between a capsule and a polyhedron
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) override;
}; };
} }

17
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
View File

@ -27,7 +27,7 @@
#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" #include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h" #include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h" #include "SAT/SATAlgorithm.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -35,7 +35,9 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra // Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius. // by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, void ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool reportContacts,
MemoryAllocator& memoryAllocator) { MemoryAllocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point // Run the SAT algorithm to find the separating axis and compute contact point
@ -47,13 +49,14 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo*
#endif #endif
// Get the last frame collision info satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, reportContacts);
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
lastFrameCollisionInfo->wasUsingSAT = true; lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingGJK = false;
}
return isColliding;
} }

4
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
View File

@ -65,7 +65,9 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra /// Compute the narrow-phase collision detection between two convex polyhedra
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) override;
}; };
} }

47
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -30,7 +30,8 @@
#include "collision/shapes/TriangleShape.h" #include "collision/shapes/TriangleShape.h"
#include "configuration.h" #include "configuration.h"
#include "utils/Profiler.h" #include "utils/Profiler.h"
#include "collision/NarrowPhaseInfo.h" #include "containers/List.h"
#include "collision/NarrowPhaseInfoBatch.h"
#include "collision/narrowphase/GJK/VoronoiSimplex.h" #include "collision/narrowphase/GJK/VoronoiSimplex.h"
#include <cassert> #include <cassert>
@ -46,10 +47,14 @@ using namespace reactphysics3d;
/// algorithm on the enlarged object to obtain a simplex polytope that contains the /// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute /// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects. /// the correct penetration depth and contact points between the enlarged objects.
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, List<GJKResult>& gjkResults, bool reportContacts) {
RP3D_PROFILE("GJKAlgorithm::testCollision()", mProfiler); RP3D_PROFILE("GJKAlgorithm::testCollision()", mProfiler);
// For each item in the batch
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
Vector3 suppA; // Support point of object A Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B Vector3 w; // Support point of Minkowski difference A-B
@ -59,15 +64,15 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
decimal prevDistSquare; decimal prevDistSquare;
bool contactFound = false; bool contactFound = false;
assert(narrowPhaseInfo->collisionShape1->isConvex()); assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->isConvex());
assert(narrowPhaseInfo->collisionShape2->isConvex()); assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1); const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2); const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// Transform a point from local space of body 2 to local // Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1) // space of body 1 (the GJK algorithm is done in local space of body 1)
@ -87,7 +92,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
VoronoiSimplex simplex; VoronoiSimplex simplex;
// Get the last collision frame info // Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// Get the previous point V (last cached separating axis) // Get the previous point V (last cached separating axis)
Vector3 v; Vector3 v;
@ -102,6 +107,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST; decimal distSquare = DECIMAL_LARGEST;
bool noIntersection = false;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
@ -120,7 +127,9 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
lastFrameCollisionInfo->gjkSeparatingAxis = v; lastFrameCollisionInfo->gjkSeparatingAxis = v;
// No intersection, we return // No intersection, we return
return GJKResult::SEPARATED; gjkResults.add(GJKResult::SEPARATED);
noIntersection = true;
break;
} }
// If the objects intersect only in the margins // If the objects intersect only in the margins
@ -170,6 +179,10 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()); } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
if (noIntersection) {
continue;
}
if (contactFound && distSquare > MACHINE_EPSILON) { if (contactFound && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
@ -188,12 +201,14 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// If the penetration depth is negative (due too numerical errors), there is no contact // If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= decimal(0.0)) { if (penetrationDepth <= decimal(0.0)) {
return GJKResult::SEPARATED; gjkResults.add(GJKResult::SEPARATED);
continue;
} }
// Do not generate a contact point with zero normal length // Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) { if (normal.lengthSquare() < MACHINE_EPSILON) {
return GJKResult::SEPARATED; gjkResults.add(GJKResult::SEPARATED);
continue;
} }
if (reportContacts) { if (reportContacts) {
@ -203,11 +218,13 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
penetrationDepth, normal); penetrationDepth, normal);
// Add a new contact point // Add a new contact point
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, pA, pB);
} }
return GJKResult::COLLIDE_IN_MARGIN; gjkResults.add(GJKResult::COLLIDE_IN_MARGIN);
continue;
} }
return GJKResult::INTERPENETRATE; gjkResults.add(GJKResult::INTERPENETRATE);
}
} }

7
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -28,16 +28,18 @@
// Libraries // Libraries
#include "decimal.h" #include "decimal.h"
#include "configuration.h"
/// ReactPhysics3D namespace /// ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
// Declarations // Declarations
class ContactManifoldInfo; class ContactManifoldInfo;
struct NarrowPhaseInfo; struct NarrowPhaseInfoBatch;
class ConvexShape; class ConvexShape;
class Profiler; class Profiler;
class VoronoiSimplex; class VoronoiSimplex;
template<typename T> class List;
// Constants // Constants
constexpr decimal REL_ERROR = decimal(1.0e-3); constexpr decimal REL_ERROR = decimal(1.0e-3);
@ -95,7 +97,8 @@ class GJKAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete; GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide. /// Compute a contact info if the two bounding volumes collide.
GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts); void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, List<GJKResult>& gjkResults, bool reportContacts);
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE

6
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -27,6 +27,7 @@
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H #define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
// Libraries // Libraries
#include "configuration.h"
/// Namespace ReactPhysics3D /// Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
@ -36,7 +37,7 @@ class Body;
class ContactManifoldInfo; class ContactManifoldInfo;
class PoolAllocator; class PoolAllocator;
class OverlappingPair; class OverlappingPair;
struct NarrowPhaseInfo; struct NarrowPhaseInfoBatch;
struct ContactPointInfo; struct ContactPointInfo;
class Profiler; class Profiler;
class MemoryAllocator; class MemoryAllocator;
@ -94,7 +95,8 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide /// Compute a contact info if the two bounding volumes collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator)=0; MemoryAllocator& memoryAllocator)=0;
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE

167
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -30,8 +30,8 @@
#include "collision/shapes/CapsuleShape.h" #include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/SphereShape.h" #include "collision/shapes/SphereShape.h"
#include "engine/OverlappingPair.h" #include "engine/OverlappingPair.h"
#include "collision/NarrowPhaseInfoBatch.h"
#include "collision/shapes/TriangleShape.h" #include "collision/shapes/TriangleShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "configuration.h" #include "configuration.h"
#include "utils/Profiler.h" #include "utils/Profiler.h"
#include <cassert> #include <cassert>
@ -52,23 +52,27 @@ SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(
} }
// Test collision between a sphere and a convex mesh // Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { void SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool reportContacts) const {
RP3D_PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler); RP3D_PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler);
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE;
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE);
// Get the capsule collision shapes // Get the capsule collision shapes
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
// Get the transform from sphere local-space to polyhedron local-space // Get the transform from sphere local-space to polyhedron local-space
const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse(); const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
@ -80,6 +84,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
// Minimum penetration depth // Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST; decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0; uint minFaceIndex = 0;
bool noContact = false;
// For each face of the convex mesh // For each face of the convex mesh
for (uint f = 0; f < polyhedron->getNbFaces(); f++) { for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
@ -90,7 +95,8 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
// If the penetration depth is negative, we have found a separating axis // If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) { if (penetrationDepth <= decimal(0.0)) {
return false; noContact = true;
break;
} }
// Check if we have found a new minimum penetration axis // Check if we have found a new minimum penetration axis
@ -100,6 +106,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
} }
} }
if (noContact) {
continue;
}
if (reportContacts) { if (reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
@ -110,19 +120,20 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld; Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld); minPenetrationDepth, normalWorld);
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
} }
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
} }
// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction // Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
@ -144,23 +155,23 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
} }
// Test collision between a capsule and a convex mesh // Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool reportContacts) const {
RP3D_PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler); RP3D_PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes // Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@ -264,7 +275,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1); narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
} }
} }
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
@ -282,14 +293,14 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld); minPenetrationDepth, normalWorld);
// Create the contact point // Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
} }
@ -362,7 +373,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool isCapsuleShape1) const {
RP3D_PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler); RP3D_PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler);
@ -427,15 +438,15 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule, isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, normalWorld); penetrationDepth, normalWorld);
// Create the contact point // Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
} }
@ -457,17 +468,19 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
} }
// Test collision between two convex polyhedrons // Test collision between two convex polyhedrons
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { void SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts) const {
RP3D_PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler); RP3D_PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape1); assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2); assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse(); const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse();
decimal minPenetrationDepth = DECIMAL_LARGEST; decimal minPenetrationDepth = DECIMAL_LARGEST;
@ -481,7 +494,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE; bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE;
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// If the last frame collision info is valid and was also using SAT algorithm // If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) { if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
@ -501,7 +514,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) { if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Return no collision without running the whole SAT algorithm // Return no collision without running the whole SAT algorithm
return false; continue;
} }
// The two shapes were overlapping in the previous frame and still seem to overlap in this one // The two shapes were overlapping in the previous frame and still seem to overlap in this one
@ -515,7 +528,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Compute the contact points between two faces of two convex polyhedra. // Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) { narrowPhaseInfoBatch, batchIndex, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
@ -523,7 +536,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// The shapes are still overlapping in the previous axis (the contact manifold is not empty). // 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 // Therefore, we can return without running the whole SAT algorithm
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
} }
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again // The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
@ -539,7 +553,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) { if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Return no collision without running the whole SAT algorithm // Return no collision without running the whole SAT algorithm
return false; continue;
} }
// The two shapes were overlapping in the previous frame and still seem to overlap in this one // The two shapes were overlapping in the previous frame and still seem to overlap in this one
@ -553,7 +567,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Compute the contact points between two faces of two convex polyhedra. // Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) { narrowPhaseInfoBatch, batchIndex, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
@ -561,7 +575,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// The shapes are still overlapping in the previous axis (the contact manifold is not empty). // 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 // Therefore, we can return without running the whole SAT algorithm
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
} }
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again // The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
@ -595,7 +610,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) { if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// We have found a separating axis without running the whole SAT algorithm // We have found a separating axis without running the whole SAT algorithm
return false; continue;
} }
// If the shapes were overlapping on the previous axis and still seem to overlap in this frame // If the shapes were overlapping on the previous axis and still seem to overlap in this frame
@ -634,21 +649,22 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
} }
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; //Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normal.getUnit(); Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normal.getUnit();
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, normalWorld); penetrationDepth, normalWorld);
// Create the contact point // Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
// The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore // The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore
// we return without running the whole SAT algorithm // we return without running the whole SAT algorithm
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
} }
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again // The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
@ -667,7 +683,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis // We have found a separating axis
return false; continue;
} }
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true; isMinPenetrationFaceNormal = true;
@ -685,7 +701,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis // We have found a separating axis
return false; continue;
} }
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true; isMinPenetrationFaceNormal = true;
@ -694,6 +710,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
isMinPenetrationFaceNormalPolyhedron1 = false; isMinPenetrationFaceNormalPolyhedron1 = false;
} }
bool separatingAxisFound = false;
// Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
@ -732,7 +750,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinEdge2Index = j; lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis // We have found a separating axis
return false; separatingAxisFound = true;
break;
} }
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
@ -750,6 +769,14 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
} }
} }
} }
if (separatingAxisFound) {
break;
}
}
if (separatingAxisFound) {
continue;
} }
// Here we know the shapes are overlapping on a given minimum separating axis. // Here we know the shapes are overlapping on a given minimum separating axis.
@ -766,7 +793,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Compute the contact points between two faces of two convex polyhedra. // Compute the contact points between two faces of two convex polyhedra.
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
minFaceIndex, narrowPhaseInfo, minPenetrationDepth); minFaceIndex, narrowPhaseInfoBatch, batchIndex, minPenetrationDepth);
// There should be clipping points here. If it is not the case, it might be // There should be clipping points here. If it is not the case, it might be
// because of a numerical issue // because of a numerical issue
@ -777,7 +804,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Return no collision // Return no collision
return false; continue;
} }
} }
@ -808,16 +835,16 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid()); normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
} }
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; //Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normal.getUnit(); Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normal.getUnit();
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld); minPenetrationDepth, normalWorld);
// Create the contact point // Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
} }
@ -827,7 +854,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index; lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
} }
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
} }
// Compute the contact points between two faces of two convex polyhedra. // Compute the contact points between two faces of two convex polyhedra.
@ -835,7 +863,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1,
const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1, const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1,
uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const { uint minFaceIndex, NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex,
decimal minPenetrationDepth) const {
RP3D_PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler); RP3D_PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler);
@ -850,8 +879,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
// Compute the world normal // Compute the world normal
Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace :
-(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); -(narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace);
// Get the reference face // Get the reference face
const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
@ -930,14 +959,14 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex);
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, outWorldNormal); penetrationDepth, outWorldNormal);
// Create a new contact point // Create a new contact point
narrowPhaseInfo->addContactPoint(outWorldNormal, penetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, outWorldNormal, penetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
} }

14
src/collision/narrowphase/SAT/SATAlgorithm.h
View File

@ -37,7 +37,7 @@ namespace reactphysics3d {
class CapsuleShape; class CapsuleShape;
class SphereShape; class SphereShape;
class ContactManifoldInfo; class ContactManifoldInfo;
struct NarrowPhaseInfo; struct NarrowPhaseInfoBatch;
class ConvexPolyhedronShape; class ConvexPolyhedronShape;
class MemoryAllocator; class MemoryAllocator;
class Profiler; class Profiler;
@ -118,7 +118,7 @@ class SATAlgorithm {
bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1, bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2, const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2,
const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex, const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex,
NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const; NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, decimal minPenetrationDepth) const;
public : public :
@ -138,24 +138,26 @@ class SATAlgorithm {
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
/// Test collision between a sphere and a convex mesh /// Test collision between a sphere and a convex mesh
bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; void testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool reportContacts) const;
/// Test collision between a capsule and a convex mesh /// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool reportContacts) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const; NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool isCapsuleShape1) const;
// This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const; const Vector3& edgeAdjacentFace2Normal) const;
/// Test collision between two convex meshes /// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; void testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts) const;
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE

33
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
View File

@ -27,7 +27,7 @@
#include "SphereVsCapsuleAlgorithm.h" #include "SphereVsCapsuleAlgorithm.h"
#include "collision/shapes/SphereShape.h" #include "collision/shapes/SphereShape.h"
#include "collision/shapes/CapsuleShape.h" #include "collision/shapes/CapsuleShape.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -35,20 +35,23 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule // 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 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius. // by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, void SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
MemoryAllocator& memoryAllocator) { bool reportContacts, MemoryAllocator& memoryAllocator) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE;
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(isSphereShape1 || narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes // Get the collision shapes
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
// Get the transform from sphere local-space to capsule local-space // Get the transform from sphere local-space to capsule local-space
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse(); const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform; const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
@ -123,17 +126,19 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
} }
if (penetrationDepth <= decimal(0.0)) { if (penetrationDepth <= decimal(0.0)) {
return false;
// No collision
continue;
} }
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
} }
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
} }
return false;
} }

4
src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
View File

@ -66,7 +66,9 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a capsule /// Compute the narrow-phase collision detection between a sphere and a capsule
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) override;
}; };
} }

45
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
View File

@ -27,7 +27,7 @@
#include "SphereVsConvexPolyhedronAlgorithm.h" #include "SphereVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h" #include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h" #include "SAT/SATAlgorithm.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -35,16 +35,8 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron // 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 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius. // by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, void SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
MemoryAllocator& memoryAllocator) { bool reportContacts, MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// First, we run the GJK algorithm // First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm; GJKAlgorithm gjkAlgorithm;
@ -55,20 +47,36 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
#endif #endif
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); List<GJKAlgorithm::GJKResult> gjkResults(memoryAllocator);
gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults, reportContacts);
// 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 ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE);
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
lastFrameCollisionInfo->wasUsingGJK = true; lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false; lastFrameCollisionInfo->wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration) // If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// Return true // Return true
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
} }
// If we have overlap even without the margins (deep penetration) // If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point // Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator); SATAlgorithm satAlgorithm(memoryAllocator);
@ -79,13 +87,14 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
#endif #endif
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true; lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding; continue;
} }
return false; resultIndex++;
}
} }

4
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
View File

@ -71,7 +71,9 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) override;
}; };
} }

30
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -26,24 +26,29 @@
// Libraries // Libraries
#include "SphereVsSphereAlgorithm.h" #include "SphereVsSphereAlgorithm.h"
#include "collision/shapes/SphereShape.h" #include "collision/shapes/SphereShape.h"
#include "collision/NarrowPhaseInfo.h" #include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, void SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
MemoryAllocator& memoryAllocator) { bool reportContacts, MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); // For each item in the batch
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE);
// Get the sphere collision shapes // Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1); const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2); const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// Compute the distance between the centers // Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@ -81,11 +86,10 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo
} }
// Create the contact info object // Create the contact info object
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
} }
return true; narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
} }
return false;
} }

4
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -65,7 +65,9 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; virtual void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) override;
}; };
} }

2
src/constraint/ContactPoint.h
View File

@ -37,8 +37,6 @@ namespace reactphysics3d {
// Declarations // Declarations
class CollisionBody; class CollisionBody;
struct NarrowPhaseInfo;
// Class ContactPoint // Class ContactPoint
/** /**
* This class represents a collision contact point between two * This class represents a collision contact point between two

1
src/engine/OverlappingPair.cpp
View File

@ -26,7 +26,6 @@
// Libraries // Libraries
#include <cassert> #include <cassert>
#include "OverlappingPair.h" #include "OverlappingPair.h"
#include "collision/NarrowPhaseInfo.h"
#include "containers/containers_common.h" #include "containers/containers_common.h"
#include "collision/ContactPointInfo.h" #include "collision/ContactPointInfo.h"

8
src/engine/OverlappingPair.h
View File

@ -38,7 +38,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Declarations // Declarations
struct NarrowPhaseInfo; struct NarrowPhaseInfoBatch;
class CollisionShape; class CollisionShape;
// Structure LastFrameCollisionInfo // Structure LastFrameCollisionInfo
@ -159,7 +159,7 @@ class OverlappingPair {
const ContactManifoldSet& getContactManifoldSet(); const ContactManifoldSet& getContactManifoldSet();
/// Add potential contact-points from narrow-phase into potential contact manifolds /// Add potential contact-points from narrow-phase into potential contact manifolds
void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo); void addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
/// Return a reference to the temporary memory allocator /// Return a reference to the temporary memory allocator
MemoryAllocator& getTemporaryAllocator(); MemoryAllocator& getTemporaryAllocator();
@ -289,8 +289,8 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint s
} }
// Create a new potential contact manifold using contact-points from narrow-phase // Create a new potential contact manifold using contact-points from narrow-phase
inline void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { inline void OverlappingPair::addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
mContactManifoldSet.addContactPoints(narrowPhaseInfo); mContactManifoldSet.addContactPoints(narrowPhaseInfoBatch, batchIndex);
} }
} }