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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

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

227
src/collision/CollisionDetection.cpp
View File

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

10
src/collision/CollisionDetection.h
View File

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

10
src/collision/ContactManifoldSet.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "ContactManifoldSet.h"
#include "NarrowPhaseInfo.h"
#include "NarrowPhaseInfoBatch.h"
#include "constraint/ContactPoint.h"
#include "ProxyShape.h"
#include "collision/ContactManifold.h"
@ -49,14 +49,14 @@ ContactManifoldSet::~ContactManifoldSet() {
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 (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
// (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
#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
// Libraries
#include "configuration.h"
namespace reactphysics3d {
// Declarations
@ -34,7 +37,7 @@ class ContactManifoldInfo;
class ProxyShape;
class MemoryAllocator;
struct WorldSettings;
struct NarrowPhaseInfo;
struct NarrowPhaseInfoBatch;
struct Vector3;
class CollisionShape;
class Transform;
@ -110,7 +113,7 @@ class ContactManifoldSet {
~ContactManifoldSet();
/// Add the contact points from the narrow phase
void addContactPoints(NarrowPhaseInfo* narrowPhaseInfo);
void addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
/// Return the first proxy shape
ProxyShape* getShape1() const;

10
src/collision/MiddlePhaseTriangleCallback.cpp
View File

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

10
src/collision/MiddlePhaseTriangleCallback.h
View File

@ -41,7 +41,7 @@ class NarrowPhaseAlgorithm;
class ProxyShape;
class MemoryAllocator;
class Profiler;
struct NarrowPhaseInfo;
struct NarrowPhaseInfoBatch;
struct Vector3;
// Class ConvexVsTriangleCallback
@ -66,8 +66,8 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
/// Pointer to the concave collision shape
const ConcaveShape* mConcaveShape;
/// Reference to the list of narrow-phase infos
List<NarrowPhaseInfo*>& mOutNarrowPhaseInfos;
/// Reference to the narrow phase info batch
NarrowPhaseInfoBatch& mOutNarrowPhaseInfoBatch;
/// Reference to the single-frame memory allocator
MemoryAllocator& mAllocator;
@ -85,11 +85,11 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
ProxyShape* concaveProxyShape,
ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
List<NarrowPhaseInfo*>& outNarrowPhaseInfos,
NarrowPhaseInfoBatch& outNarrowPhaseInfoBatch,
MemoryAllocator& allocator)
:mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
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
#include "NarrowPhaseInfo.h"
#include "NarrowPhaseInfoBatch.h"
#include "ContactPointInfo.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h"
@ -32,61 +32,63 @@
using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(overlappingPair->getTemporaryAllocator()), collisionShapeAllocator(shapeAllocator) {
NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator)
: mMemoryAllocator(allocator), overlappingPairs(allocator), collisionShapes1(allocator), collisionShapes2(allocator),
shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator),
isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator) {
// Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
}
// Destructor
NarrowPhaseInfo::~NarrowPhaseInfo() {
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));
NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() {
clear();
}
// 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
void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator();
// Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the list of contact points
contactPoints.add(contactPointInfo);
contactPoints[index].add(contactPointInfo);
}
// Reset the remaining contact points
void NarrowPhaseInfo::resetContactPoints() {
void NarrowPhaseInfoBatch::resetContactPoints(uint index) {
// Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator();
// 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
contactPoint->~ContactPointInfo();
@ -95,5 +97,34 @@ void NarrowPhaseInfo::resetContactPoints() {
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();
}

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

@ -23,8 +23,8 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_H
#define REACTPHYSICS3D_NARROW_PHASE_INFO_H
#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_BATCH_H
#define REACTPHYSICS3D_NARROW_PHASE_INFO_BATCH_H
// Libraries
#include "engine/OverlappingPair.h"
@ -38,58 +38,82 @@ struct LastFrameCollisionInfo;
class ContactManifoldInfo;
struct ContactPointInfo;
// Class NarrowPhaseInfo
// Struct NarrowPhaseInfoBatch
/**
* This structure regroups different things about a collision shape. This is
* used to pass information about a collision shape to a collision algorithm.
* This abstract structure collects all the potential collisions from the middle-phase 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:
/// Broadphase overlapping pair
OverlappingPair* overlappingPair;
/// List of Broadphase overlapping pairs
List<OverlappingPair*> overlappingPairs;
/// Pointer to the first collision shape to test collision with
CollisionShape* collisionShape1;
/// List of pointers to the first collision shapes to test collision with
List<CollisionShape*> collisionShapes1;
/// Pointer to the second collision shape to test collision with
CollisionShape* collisionShape2;
/// List of pointers to the second collision shapes to test collision with
List<CollisionShape*> collisionShapes2;
/// Transform that maps from collision shape 1 local-space to world-space
Transform shape1ToWorldTransform;
/// List of transforms that maps from collision shape 1 local-space to world-space
List<Transform> shape1ToWorldTransforms;
/// Transform that maps from collision shape 2 local-space to world-space
Transform shape2ToWorldTransform;
/// List of transforms that maps from collision shape 2 local-space to world-space
List<Transform> shape2ToWorldTransforms;
/// Result of the narrow-phase collision detection test
List<bool> isColliding;
/// 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)
MemoryAllocator& collisionShapeAllocator;
/// Memory allocators for the collision shape (Used to release TriangleShape memory in destructor)
List<MemoryAllocator*> collisionShapeAllocators;
/// 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,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
/// 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);
/// 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
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
inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const {
return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId());
inline LastFrameCollisionInfo* NarrowPhaseInfoBatch::getLastFrameCollisionInfo(uint index) const {
return overlappingPairs[index]->getLastFrameCollisionInfo(collisionShapes1[index]->getId(), collisionShapes2[index]->getId());
}
}

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

@ -26,7 +26,7 @@
// Libraries
#include "CapsuleVsCapsuleAlgorithm.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -34,18 +34,22 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two capsules
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
void CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// 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
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.
// 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
@ -139,14 +143,15 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
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 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2;
decimal penetrationDepth = sumRadius - closestPointsDistance;
// 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)
@ -197,10 +202,10 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->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
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
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 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
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
#include "NarrowPhaseAlgorithm.h"
#include "configuration.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
struct NarrowPhaseInfoBatch;
class Body;
class ContactPoint;
@ -65,7 +67,9 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// 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 "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/NarrowPhaseInfoBatch.h"
#include "collision/ContactPointInfo.h"
#include <cassert>
@ -39,7 +39,9 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
void CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool reportContacts,
MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm
@ -53,43 +55,51 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
#endif
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// Run the GJK algorithm
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->wasUsingSAT = false;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// 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) {
bool noContact = false;
// 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
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK
assert(narrowPhaseInfo->contactPoints.size() > 0);
ContactPointInfo*& contactPoint = narrowPhaseInfo->contactPoints[0];
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 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
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
// For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
@ -109,9 +119,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// 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;
// 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,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
if (!contactsFound) {
return false;
noContact = true;
narrowPhaseInfoBatch.isColliding[batchIndex] = false;
break;
}
break;
}
}
if (noContact) {
continue;
}
}
lastFrameCollisionInfo->wasUsingSAT = false;
lastFrameCollisionInfo->wasUsingGJK = false;
// Return true
return true;
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
// 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
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
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;
/// 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 "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -35,7 +35,9 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
void ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
bool reportContacts,
MemoryAllocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point
@ -47,13 +49,14 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo*
#endif
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, reportContacts);
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->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;
/// 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 "configuration.h"
#include "utils/Profiler.h"
#include "collision/NarrowPhaseInfo.h"
#include "containers/List.h"
#include "collision/NarrowPhaseInfoBatch.h"
#include "collision/narrowphase/GJK/VoronoiSimplex.h"
#include <cassert>
@ -46,10 +47,14 @@ using namespace reactphysics3d;
/// 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
/// 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);
// For each item in the batch
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B
@ -59,15 +64,15 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
decimal prevDistSquare;
bool contactFound = false;
assert(narrowPhaseInfo->collisionShape1->isConvex());
assert(narrowPhaseInfo->collisionShape2->isConvex());
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->isConvex());
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// 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)
@ -87,7 +92,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
VoronoiSimplex simplex;
// Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// Get the previous point V (last cached separating axis)
Vector3 v;
@ -102,6 +107,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
bool noIntersection = false;
do {
// 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;
// No intersection, we return
return GJKResult::SEPARATED;
gjkResults.add(GJKResult::SEPARATED);
noIntersection = true;
break;
}
// 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());
if (noIntersection) {
continue;
}
if (contactFound && distSquare > MACHINE_EPSILON) {
// 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 (penetrationDepth <= decimal(0.0)) {
return GJKResult::SEPARATED;
gjkResults.add(GJKResult::SEPARATED);
continue;
}
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
return GJKResult::SEPARATED;
gjkResults.add(GJKResult::SEPARATED);
continue;
}
if (reportContacts) {
@ -203,11 +218,13 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
penetrationDepth, normal);
// 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
#include "decimal.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class ContactManifoldInfo;
struct NarrowPhaseInfo;
struct NarrowPhaseInfoBatch;
class ConvexShape;
class Profiler;
class VoronoiSimplex;
template<typename T> class List;
// Constants
constexpr decimal REL_ERROR = decimal(1.0e-3);
@ -95,7 +97,8 @@ class GJKAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// 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

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

@ -27,6 +27,7 @@
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
// Libraries
#include "configuration.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -36,7 +37,7 @@ class Body;
class ContactManifoldInfo;
class PoolAllocator;
class OverlappingPair;
struct NarrowPhaseInfo;
struct NarrowPhaseInfoBatch;
struct ContactPointInfo;
class Profiler;
class MemoryAllocator;
@ -94,7 +95,8 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// 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;
#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/SphereShape.h"
#include "engine/OverlappingPair.h"
#include "collision/NarrowPhaseInfoBatch.h"
#include "collision/shapes/TriangleShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "configuration.h"
#include "utils/Profiler.h"
#include <cassert>
@ -52,23 +52,27 @@ SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(
}
// 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);
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE;
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 capsule collision shapes
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
// Get the transform from sphere local-space to polyhedron local-space
const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
@ -80,6 +84,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
bool noContact = false;
// For each face of the convex mesh
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 (penetrationDepth <= decimal(0.0)) {
return false;
noContact = true;
break;
}
// Check if we have found a new minimum penetration axis
@ -100,6 +106,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
}
}
if (noContact) {
continue;
}
if (reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
@ -110,19 +120,20 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
// 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 ? contactPointPolyhedronLocal : contactPointSphereLocal,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld);
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
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
@ -144,23 +155,23 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
}
// 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);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@ -264,7 +275,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
}
}
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;
// 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 ? closestPointPolyhedronEdge : contactPointCapsule,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
}
@ -362,7 +373,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool isCapsuleShape1) const {
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
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
}
@ -457,17 +468,19 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// 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);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape1);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
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();
decimal minPenetrationDepth = DECIMAL_LARGEST;
@ -481,7 +494,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
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 (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
@ -501,7 +514,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// 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
@ -515,7 +528,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) {
narrowPhaseInfoBatch, batchIndex, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = 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).
// 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
@ -539,7 +553,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// 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
@ -553,7 +567,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) {
narrowPhaseInfoBatch, batchIndex, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = 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).
// 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
@ -595,7 +610,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// 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
@ -634,21 +649,22 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
}
//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
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
// The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore
// 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
@ -667,7 +683,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
continue;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
@ -685,7 +701,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
continue;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
@ -694,6 +710,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
isMinPenetrationFaceNormalPolyhedron1 = false;
}
bool separatingAxisFound = false;
// 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) {
@ -732,7 +750,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis
return false;
separatingAxisFound = true;
break;
}
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.
@ -766,7 +793,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Compute the contact points between two faces of two convex polyhedra.
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
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
// because of a numerical issue
@ -777,7 +804,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Return no collision
return false;
continue;
}
}
@ -808,16 +835,16 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//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
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
@ -827,7 +854,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
}
return true;
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
}
// Compute the contact points between two faces of two convex polyhedra.
@ -835,7 +863,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1,
const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
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);
@ -850,8 +879,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
// Compute the world normal
Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
-(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace :
-(narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace);
// Get the reference face
const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
@ -930,14 +959,14 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex);
// 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 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, outWorldNormal);
// Create a new contact point
narrowPhaseInfo->addContactPoint(outWorldNormal, penetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, outWorldNormal, penetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}

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

@ -37,7 +37,7 @@ namespace reactphysics3d {
class CapsuleShape;
class SphereShape;
class ContactManifoldInfo;
struct NarrowPhaseInfo;
struct NarrowPhaseInfoBatch;
class ConvexPolyhedronShape;
class MemoryAllocator;
class Profiler;
@ -118,7 +118,7 @@ class SATAlgorithm {
bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2,
const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex,
NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const;
NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, decimal minPenetrationDepth) const;
public :
@ -138,24 +138,26 @@ class SATAlgorithm {
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
/// 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
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
bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
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
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const;
/// 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

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

@ -27,7 +27,7 @@
#include "SphereVsCapsuleAlgorithm.h"
#include "collision/shapes/SphereShape.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -35,20 +35,23 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
void SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
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
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
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
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
@ -123,17 +126,19 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
}
if (penetrationDepth <= decimal(0.0)) {
return false;
// No collision
continue;
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
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;
/// 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 "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -35,16 +35,8 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, 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();
void SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
bool reportContacts, MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
@ -55,20 +47,36 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
#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->wasUsingSAT = false;
// 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;
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
// 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
SATAlgorithm satAlgorithm(memoryAllocator);
@ -79,13 +87,14 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
#endif
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
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;
/// 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
#include "SphereVsSphereAlgorithm.h"
#include "collision/shapes/SphereShape.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/NarrowPhaseInfoBatch.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
void SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
bool reportContacts, MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// For each item in the batch
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
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@ -81,11 +86,10 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo
}
// 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;
/// 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
class CollisionBody;
struct NarrowPhaseInfo;
// Class ContactPoint
/**
* This class represents a collision contact point between two

1
src/engine/OverlappingPair.cpp
View File

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

8
src/engine/OverlappingPair.h
View File

@ -38,7 +38,7 @@
namespace reactphysics3d {
// Declarations
struct NarrowPhaseInfo;
struct NarrowPhaseInfoBatch;
class CollisionShape;
// Structure LastFrameCollisionInfo
@ -159,7 +159,7 @@ class OverlappingPair {
const ContactManifoldSet& getContactManifoldSet();
/// 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
MemoryAllocator& getTemporaryAllocator();
@ -289,8 +289,8 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint s
}
// Create a new potential contact manifold using contact-points from narrow-phase
inline void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) {
mContactManifoldSet.addContactPoints(narrowPhaseInfo);
inline void OverlappingPair::addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
mContactManifoldSet.addContactPoints(narrowPhaseInfoBatch, batchIndex);
}
}