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,17 +582,14 @@ 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,17 +673,14 @@ 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;

14
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,
isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape,
isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(),
mAllocator);
mOutNarrowPhaseInfos.add(narrowPhaseInfo);
mOutNarrowPhaseInfoBatch.addNarrowPhaseInfo(mOverlappingPair,
isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape,
isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
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) {
}

87
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() {
NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() {
clear();
}
assert(contactPoints.size() == 0);
// 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) {
// 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));
}
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());
}
}

324
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,195 +34,199 @@ 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++) {
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
// Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
// Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() *
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
// Compute the end-points of the inner segment of the second capsule
const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
// The two inner capsule segments
const Vector3 seg1 = capsule1SegB - capsule1SegA;
const Vector3 seg2 = capsule2SegB - capsule2SegA;
// Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
// Compute the sum of the radius of the two capsules (virtual spheres)
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// Compute the end-points of the inner segment of the second capsule
const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
// If the two capsules are parallel (we create two contact points)
bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
if (areCapsuleInnerSegmentsParralel) {
// The two inner capsule segments
const Vector3 seg1 = capsule1SegB - capsule1SegA;
const Vector3 seg2 = capsule2SegB - capsule2SegA;
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsPerpendicularDistance >= sumRadius) {
// Compute the sum of the radius of the two capsules (virtual spheres)
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// 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;
}
// If the two capsules are parallel (we create two contact points)
bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
if (areCapsuleInnerSegmentsParralel) {
// Compute the planes that goes through the extreme points of the inner segment of capsule 1
decimal d1 = seg1.dot(capsule1SegA);
decimal d2 = -seg1.dot(capsule1SegB);
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsPerpendicularDistance >= sumRadius) {
// Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
// segment of capsule 1
decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
// 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.
continue;
}
// If the segments were overlapping (the clip segment is valid)
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
// Compute the planes that goes through the extreme points of the inner segment of capsule 1
decimal d1 = seg1.dot(capsule1SegA);
decimal d2 = -seg1.dot(capsule1SegB);
// Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
// segment of capsule 1
decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
// If the segments were overlapping (the clip segment is valid)
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
if (reportContacts) {
// Clip the inner segment of capsule 2
if (t1 > decimal(1.0)) t1 = decimal(1.0);
const Vector3 clipPointA = capsule2SegB - t1 * seg2;
if (t2 > decimal(1.0)) t2 = decimal(1.0);
const Vector3 clipPointB = capsule2SegA + t2 * seg2;
// Project point capsule2SegA onto line of innner segment of capsule 1
const Vector3 seg1Normalized = seg1.getUnit();
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
Vector3 normalCapsule2SpaceNormalized;
Vector3 segment1ToSegment2;
// If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
// Compute a perpendicular vector from segment 1 to segment 2
segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
}
else { // If the capsule inner segments are overlapping (degenerate case)
// We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
// any vector that is orthogonal to the inner capsule segments.
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
Vector3 seg2Normalized = seg2.getUnit();
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
segment1ToSegment2.setToZero();
// We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
}
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
}
// Compute the closest points between the two inner capsule segments
Vector3 closestPointCapsule1Seg;
Vector3 closestPointCapsule2Seg;
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
closestPointCapsule1Seg, closestPointCapsule2Seg);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius) {
if (reportContacts) {
// Clip the inner segment of capsule 2
if (t1 > decimal(1.0)) t1 = decimal(1.0);
const Vector3 clipPointA = capsule2SegB - t1 * seg2;
if (t2 > decimal(1.0)) t2 = decimal(1.0);
const Vector3 clipPointB = capsule2SegA + t2 * seg2;
// If the distance between the inner segments is not zero
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
// Project point capsule2SegA onto line of innner segment of capsule 1
const Vector3 seg1Normalized = seg1.getUnit();
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
Vector3 normalCapsule2SpaceNormalized;
Vector3 segment1ToSegment2;
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
// If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2;
// Compute a perpendicular vector from segment 1 to segment 2
segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
}
else { // If the capsule inner segments are overlapping (degenerate case)
decimal penetrationDepth = sumRadius - closestPointsDistance;
// We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
// any vector that is orthogonal to the inner capsule segments.
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // The segment are overlapping (degenerate case)
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
// If the capsule segments are parralel
if (areCapsuleInnerSegmentsParralel) {
Vector3 seg2Normalized = seg2.getUnit();
// The segment are parallel, not overlapping and their distance is zero.
// Therefore, the capsules are just touching at the top of their inner segments
decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare();
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB;
Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
normalCapsuleSpace2.normalize();
segment1ToSegment2.setToZero();
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
// We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
}
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // If the capsules inner segments are not parallel
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
// We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross
// product between the two segments.
Vector3 normalCapsuleSpace2 = seg1.cross(seg2);
normalCapsuleSpace2.normalize();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
// Compute the contact points on both shapes
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
}
}
return true;
}
}
// Compute the closest points between the two inner capsule segments
Vector3 closestPointCapsule1Seg;
Vector3 closestPointCapsule2Seg;
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
closestPointCapsule1Seg, closestPointCapsule2Seg);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius) {
if (reportContacts) {
// If the distance between the inner segments is not zero
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // The segment are overlapping (degenerate case)
// If the capsule segments are parralel
if (areCapsuleInnerSegmentsParralel) {
// The segment are parallel, not overlapping and their distance is zero.
// Therefore, the capsules are just touching at the top of their inner segments
decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare();
Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB;
Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
normalCapsuleSpace2.normalize();
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // If the capsules inner segments are not parallel
// We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross
// product between the two segments.
Vector3 normalCapsuleSpace2 = seg1.cross(seg2);
normalCapsuleSpace2.normalize();
// Compute the contact points on both shapes
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
}
}
return true;
}
return false;
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
}
}

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;
};
}

184
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,114 +55,128 @@ 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++) {
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
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) {
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 (reportContacts) {
// If we have found a contact point inside the margins (shallow penetration)
if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// 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)
if (reportContacts) {
// Get the contact point created by GJK
assert(narrowPhaseInfo->contactPoints.size() > 0);
ContactPointInfo*& contactPoint = narrowPhaseInfo->contactPoints[0];
bool noContact = false;
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
// 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 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);
// Get the contact point created by GJK
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
ContactPointInfo*& contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][0];
// For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE;
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
// Get the collision shapes
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]);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
// For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
capsuleInnerSegmentDirection.normalize();
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
// is in direction of the contact normal (from the polyhedron point of view).
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
narrowPhaseInfo->resetContactPoints();
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
// Compute the end-points of the inner segment of the capsule
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
capsuleInnerSegmentDirection.normalize();
// Convert the inner capsule segment points into the polyhedron local-space
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
// is in direction of the contact normal (from the polyhedron point of view).
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
if (isCapsuleShape1) {
faceNormalWorld = -faceNormalWorld;
// Remove the previous contact point computed by GJK
narrowPhaseInfoBatch.resetContactPoints(batchIndex);
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
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
// Convert the inner capsule segment points into the polyhedron local-space
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
if (isCapsuleShape1) {
faceNormalWorld = -faceNormalWorld;
}
// Compute and create two contact points
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
if (!contactsFound) {
noContact = true;
narrowPhaseInfoBatch.isColliding[batchIndex] = false;
break;
}
break;
}
}
// Compute and create two contact points
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
if (!contactsFound) {
return false;
}
break;
if (noContact) {
continue;
}
}
lastFrameCollisionInfo->wasUsingSAT = false;
lastFrameCollisionInfo->wasUsingGJK = false;
// Return true
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
lastFrameCollisionInfo->wasUsingSAT = false;
lastFrameCollisionInfo->wasUsingGJK = false;
// If we have overlap even without the margins (deep penetration)
if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Return true
return true;
// Run the SAT algorithm to find the separating axis and compute contact point
narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
}
resultIndex++;
}
// If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
return false;
}

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;
};
}

19
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++) {
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
return isColliding;
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;
}
}

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;
};
}

275
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,168 +47,184 @@ 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);
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B
decimal vDotw;
decimal prevDistSquare;
bool contactFound = false;
// For each item in the batch
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfo->collisionShape1->isConvex());
assert(narrowPhaseInfo->collisionShape2->isConvex());
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B
decimal vDotw;
decimal prevDistSquare;
bool contactFound = false;
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->isConvex());
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->isConvex());
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.collisionShapes2[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)
Transform transform1Inverse = transform1.getInverse();
Transform body2Tobody1 = transform1Inverse * transform2;
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
// Quaternion that transform a direction from local
// space of body 1 into local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
// 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)
Transform transform1Inverse = transform1.getInverse();
Transform body2Tobody1 = transform1Inverse * transform2;
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
assert(margin > decimal(0.0));
// Quaternion that transform a direction from local
// space of body 1 into local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
// Create a simplex set
VoronoiSimplex simplex;
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
assert(margin > decimal(0.0));
// Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// Create a simplex set
VoronoiSimplex simplex;
// Get the previous point V (last cached separating axis)
Vector3 v;
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
v = lastFrameCollisionInfo->gjkSeparatingAxis;
assert(v.lengthSquare() > decimal(0.000001));
}
else {
v.setAllValues(0, 1, 0);
}
// Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
do {
// Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v);
suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
lastFrameCollisionInfo->gjkSeparatingAxis = v;
// No intersection, we return
return GJKResult::SEPARATED;
// Get the previous point V (last cached separating axis)
Vector3 v;
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
v = lastFrameCollisionInfo->gjkSeparatingAxis;
assert(v.lengthSquare() > decimal(0.000001));
}
else {
v.setAllValues(0, 1, 0);
}
// If the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
// Contact point has been found
contactFound = true;
break;
}
bool noIntersection = false;
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
do {
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
// Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v);
suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Contact point has been found
contactFound = true;
break;
}
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fails
if (!simplex.computeClosestPoint(v)) {
vDotw = v.dot(w);
// Contact point has been found
contactFound = true;
break;
}
// If the enlarge objects (with margins) do not intersect
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
// Cache the current separating axis for frame coherence
lastFrameCollisionInfo->gjkSeparatingAxis = v;
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
// No intersection, we return
gjkResults.add(GJKResult::SEPARATED);
noIntersection = true;
break;
}
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
// If the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Contact point has been found
contactFound = true;
break;
}
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
// Contact point has been found
contactFound = true;
break;
}
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fails
if (!simplex.computeClosestPoint(v)) {
// Contact point has been found
contactFound = true;
break;
}
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
// Contact point has been found
contactFound = true;
break;
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.lengthSquare();
// Contact point has been found
contactFound = true;
break;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
if (noIntersection) {
continue;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
if (contactFound && distSquare > MACHINE_EPSILON) {
if (contactFound && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = std::sqrt(distSquare);
assert(dist > decimal(0.0));
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = std::sqrt(distSquare);
assert(dist > decimal(0.0));
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= decimal(0.0)) {
gjkResults.add(GJKResult::SEPARATED);
continue;
}
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= decimal(0.0)) {
return GJKResult::SEPARATED;
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
gjkResults.add(GJKResult::SEPARATED);
continue;
}
if (reportContacts) {
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
penetrationDepth, normal);
// Add a new contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, pA, pB);
}
gjkResults.add(GJKResult::COLLIDE_IN_MARGIN);
continue;
}
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
return GJKResult::SEPARATED;
}
if (reportContacts) {
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
penetrationDepth, normal);
// Add a new contact point
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
}
return GJKResult::COLLIDE_IN_MARGIN;
gjkResults.add(GJKResult::INTERPENETRATE);
}
return 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

833
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,77 +52,88 @@ 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;
// 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);
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);
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
// Get the capsule collision shapes
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]);
// Get the transform from sphere local-space to polyhedron local-space
const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
const Transform sphereToPolyhedronSpaceTransform = worldToPolyhedronTransform * sphereToWorldTransform;
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
// Transform the center of the sphere into the local-space of the convex polyhedron
const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition();
// Get the transform from sphere local-space to polyhedron local-space
const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
const Transform sphereToPolyhedronSpaceTransform = worldToPolyhedronTransform * sphereToWorldTransform;
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
// Transform the center of the sphere into the local-space of the convex polyhedron
const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition();
// For each face of the convex mesh
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
bool noContact = false;
// Compute the penetration depth of the shapes along the face normal direction
decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
// For each face of the convex mesh
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
// Compute the penetration depth of the shapes along the face normal direction
decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
return false;
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
noContact = true;
break;
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = f;
}
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = f;
if (noContact) {
continue;
}
if (reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal;
Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius());
Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld);
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
if (reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal;
Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius());
Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
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,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
}
return 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,377 +468,394 @@ 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 Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse();
const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
bool isMinPenetrationFaceNormal = false;
bool isMinPenetrationFaceNormalPolyhedron1 = false;
uint minSeparatingEdge1Index = 0;
uint minSeparatingEdge2Index = 0;
Vector3 separatingEdge1A, separatingEdge1B;
Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE;
const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse();
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
bool isMinPenetrationFaceNormal = false;
bool isMinPenetrationFaceNormalPolyhedron1 = false;
uint minSeparatingEdge1Index = 0;
uint minSeparatingEdge2Index = 0;
Vector3 separatingEdge1A, separatingEdge1B;
Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE;
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
// axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
// the shapes are still separated along this axis, we directly exit with no collision.
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
// If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 1
if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) {
// We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
// axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
// the shapes are still separated along this axis, we directly exit with no collision.
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
lastFrameCollisionInfo->satMinAxisFaceIndex);
// If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 1
if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) {
// If the previous axis was a separating axis and is still a separating axis in this frame
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
lastFrameCollisionInfo->satMinAxisFaceIndex);
// Return no collision without running the whole SAT algorithm
return false;
}
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = true;
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// 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;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 2
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
lastFrameCollisionInfo->satMinAxisFaceIndex);
// If the previous axis was a separating axis and is still a separating axis in this frame
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Return no collision without running the whole SAT algorithm
return false;
}
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = false;
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// 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;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
// Compute the penetration depth along the previous axis
const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
// If the shapes were not overlapping in the previous frame and are still not
// overlapping in the current one
// If the previous axis was a separating axis and is still a separating axis in this frame
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// We have found a separating axis without running the whole SAT algorithm
return false;
// Return no collision without running the whole SAT algorithm
continue;
}
// If the shapes were overlapping on the previous axis and still seem to overlap in this frame
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
computeClosestPointBetweenTwoSegments(edge1A, edge1B, edge2A, edge2B,
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = true;
// Here we try to project the closest point on edge1 onto the segment of edge 2 to see if
// the projected point falls onto the segment. We also try to project the closest point
// on edge 2 to see if it falls onto the segment of edge 1. If one of the point does not
// fall onto the opposite segment, it means the edges are not colliding (the contact manifold
// is empty). Therefore, we need to run the whole SAT algorithm again.
const Vector3 vec1 = closestPointPolyhedron1Edge - edge2A;
const Vector3 vec2 = closestPointPolyhedron2Edge - edge1A;
const decimal edge1LengthSquare = edge1Direction.lengthSquare();
const decimal edge2LengthSquare = edge2Direction.lengthSquare();
decimal t1 = vec1.dot(edge2Direction) / edge2LengthSquare;
decimal t2 = vec2.dot(edge1Direction) / edge1LengthSquare;
if (t1 >= decimal(0.0) && t1 <= decimal(1) && t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfoBatch, batchIndex, minPenetrationDepth)) {
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Compute the world normal
// We use the direction from the centroid to the edge of the shape that is not a triangle
// to avoid possible degeneracies when axis direction is orthogonal to triangle normal
Vector3 normal;
if (isShape1Triangle) {
normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
}
else {
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normal.getUnit();
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
penetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(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;
// The shapes are still overlapping in the previous axis (the contact manifold is not empty).
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
}
}
else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 2
// Test all the face normals of the polyhedron 1 for separating axis
uint faceIndex;
decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
lastFrameCollisionInfo->satMinAxisFaceIndex);
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// If the previous axis was a separating axis and is still a separating axis in this frame
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
// Test all the face normals of the polyhedron 2 for separating axis
penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = 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) {
// Get an edge of polyhedron 1
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
// Get an edge of polyhedron 2
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j);
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
// Compute the penetration depth
const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
if (penetrationDepth <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = i;
lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis
return false;
// Return no collision without running the whole SAT algorithm
continue;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
minSeparatingEdge1Index = i;
minSeparatingEdge2Index = j;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfoBatch, batchIndex, minPenetrationDepth)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// The shapes are still overlapping in the previous axis (the contact manifold is not empty).
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
// Compute the penetration depth along the previous axis
const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
// If the shapes were not overlapping in the previous frame and are still not
// overlapping in the current one
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// We have found a separating axis without running the whole SAT algorithm
continue;
}
// If the shapes were overlapping on the previous axis and still seem to overlap in this frame
if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
computeClosestPointBetweenTwoSegments(edge1A, edge1B, edge2A, edge2B,
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
// Here we try to project the closest point on edge1 onto the segment of edge 2 to see if
// the projected point falls onto the segment. We also try to project the closest point
// on edge 2 to see if it falls onto the segment of edge 1. If one of the point does not
// fall onto the opposite segment, it means the edges are not colliding (the contact manifold
// is empty). Therefore, we need to run the whole SAT algorithm again.
const Vector3 vec1 = closestPointPolyhedron1Edge - edge2A;
const Vector3 vec2 = closestPointPolyhedron2Edge - edge1A;
const decimal edge1LengthSquare = edge1Direction.lengthSquare();
const decimal edge2LengthSquare = edge2Direction.lengthSquare();
decimal t1 = vec1.dot(edge2Direction) / edge2LengthSquare;
decimal t2 = vec2.dot(edge1Direction) / edge1LengthSquare;
if (t1 >= decimal(0.0) && t1 <= decimal(1) && t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
// We use the direction from the centroid to the edge of the shape that is not a triangle
// to avoid possible degeneracies when axis direction is orthogonal to triangle normal
Vector3 normal;
if (isShape1Triangle) {
normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
}
else {
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
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(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
penetrationDepth, normalWorld);
// Create the contact point
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
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
}
}
}
// Here we know the shapes are overlapping on a given minimum separating axis.
// Now, we will clip the shapes along this axis to find the contact points
// Test all the face normals of the polyhedron 1 for separating axis
uint faceIndex;
decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
assert(minPenetrationDepth > decimal(0.0));
assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal);
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// If the minimum separating axis is a face normal
if (isMinPenetrationFaceNormal) {
if (reportContacts) {
// Compute the contact points between two faces of two convex polyhedra.
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
minFaceIndex, narrowPhaseInfo, minPenetrationDepth);
// There should be clipping points here. If it is not the case, it might be
// because of a numerical issue
if (!contactsFound) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Return no collision
return false;
}
// We have found a separating axis
continue;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
// Test all the face normals of the polyhedron 2 for separating axis
penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
if (reportContacts) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
// We use the direction from the centroid to the edge of the shape that is not a triangle
// to avoid possible degeneracies when axis direction is orthogonal to triangle normal
Vector3 normal;
if (isShape1Triangle) {
normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
}
else {
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normal.getUnit();
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
// We have found a separating axis
continue;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = false;
}
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index;
lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
}
bool separatingAxisFound = false;
return true;
// 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) {
// Get an edge of polyhedron 1
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
// Get an edge of polyhedron 2
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j);
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
// Compute the penetration depth
const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
if (penetrationDepth <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = i;
lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis
separatingAxisFound = true;
break;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
minSeparatingEdge1Index = i;
minSeparatingEdge2Index = j;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
}
}
}
if (separatingAxisFound) {
break;
}
}
if (separatingAxisFound) {
continue;
}
// Here we know the shapes are overlapping on a given minimum separating axis.
// Now, we will clip the shapes along this axis to find the contact points
assert(minPenetrationDepth > decimal(0.0));
assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal);
// If the minimum separating axis is a face normal
if (isMinPenetrationFaceNormal) {
if (reportContacts) {
// Compute the contact points between two faces of two convex polyhedra.
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
minFaceIndex, narrowPhaseInfoBatch, batchIndex, minPenetrationDepth);
// There should be clipping points here. If it is not the case, it might be
// because of a numerical issue
if (!contactsFound) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Return no collision
continue;
}
}
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
if (reportContacts) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
// We use the direction from the centroid to the edge of the shape that is not a triangle
// to avoid possible degeneracies when axis direction is orthogonal to triangle normal
Vector3 normal;
if (isShape1Triangle) {
normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
}
else {
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
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(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex],
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index;
lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
}
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

151
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,105 +35,110 @@ 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;
// 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);
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
assert(isSphereShape1 || narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE);
// 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 worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
// Get the collision shapes
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]);
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
// Get the transform from sphere local-space to capsule local-space
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;
// Compute the end-points of the inner segment of the capsule
const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5);
const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0);
const Vector3 capsuleSegB(0, capsuleHalfHeight, 0);
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
// Compute the point on the inner capsule segment that is the closes to center of sphere
const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter);
// Compute the end-points of the inner segment of the capsule
const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5);
const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0);
const Vector3 capsuleSegB(0, capsuleHalfHeight, 0);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter);
const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare();
// Compute the point on the inner capsule segment that is the closes to center of sphere
const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter);
// Compute the sum of the radius of the sphere and the capsule (virtual sphere)
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
// Compute the distance between the sphere center and the closest point on the segment
Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter);
const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare();
decimal penetrationDepth;
Vector3 normalWorld;
Vector3 contactPointSphereLocal;
Vector3 contactPointCapsuleLocal;
// Compute the sum of the radius of the sphere and the capsule (virtual sphere)
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
if (reportContacts) {
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
// If the sphere center is not on the capsule inner segment
if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
decimal penetrationDepth;
Vector3 normalWorld;
Vector3 contactPointSphereLocal;
Vector3 contactPointCapsuleLocal;
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
if (reportContacts) {
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
// If the sphere center is not on the capsule inner segment
if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
penetrationDepth = sumRadius - sphereSegmentDistance;
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
if (!isSphereShape1) {
normalWorld = -normalWorld;
}
}
else { // If the sphere center is on the capsule inner segment (degenerate case)
normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
// We take any direction that is orthogonal to the inner capsule segment as a contact normal
penetrationDepth = sumRadius - sphereSegmentDistance;
// Capsule inner segment
Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit();
if (!isSphereShape1) {
normalWorld = -normalWorld;
}
}
else { // If the sphere center is on the capsule inner segment (degenerate case)
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
// We take any direction that is orthogonal to the inner capsule segment as a contact normal
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2))
// Capsule inner segment
Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit();
penetrationDepth = sumRadius;
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
// We choose as a contact normal, any direction that is perpendicular to the inner capsule segment
Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2);
normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2))
// Compute the two local contact points
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius());
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
}
penetrationDepth = sumRadius;
if (penetrationDepth <= decimal(0.0)) {
return false;
// We choose as a contact normal, any direction that is perpendicular to the inner capsule segment
Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2);
normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
// Compute the two local contact points
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius());
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
}
if (penetrationDepth <= decimal(0.0)) {
// No collision
continue;
}
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
return true;
}
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;
};
}

71
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,57 +35,66 @@ 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;
#ifdef IS_PROFILING_ACTIVE
gjkAlgorithm.setProfiler(mProfiler);
gjkAlgorithm.setProfiler(mProfiler);
#endif
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
List<GJKAlgorithm::GJKResult> gjkResults(memoryAllocator);
gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
// For each item in the batch
uint resultIndex=0;
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
// Return true
return true;
}
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);
// If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.getLastFrameCollisionInfo(batchIndex);
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration)
if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// Return true
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
continue;
}
// If we have overlap even without the margins (deep penetration)
if (gjkResults[resultIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
satAlgorithm.setProfiler(mProfiler);
satAlgorithm.setProfiler(mProfiler);
#endif
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
continue;
}
resultIndex++;
}
return false;
}

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;
};
}

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

@ -26,66 +26,70 @@
// 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) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
void SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
bool reportContacts, MemoryAllocator& memoryAllocator) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
// For each item in the batch
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE);
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfoBatch.collisionShapes1[batchIndex]);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfoBatch.collisionShapes2[batchIndex]);
if (reportContacts) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// If the two sphere centers are not at the same position
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
if (reportContacts) {
intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal);
intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
// If the two sphere centers are not at the same position
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal);
intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
}
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
}
return 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

2
src/containers/List.h
View File

@ -39,7 +39,7 @@ namespace reactphysics3d {
// Class List
/**
* This class represents a simple generic list with custom memory allocator.
*/
*/
template<typename T>
class List {

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);
}
}