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

Refactor the way to create the contact manifolds and contact points

Browse Source
This commit is contained in:
Daniel Chappuis 2017-07-30 22:14:46 +02:00
parent 6eec956eb0
commit 8b82c4ac81
50 changed files with 1558 additions and 767 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -132,6 +132,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/collision/NarrowPhaseInfo.h"
"src/collision/NarrowPhaseInfo.cpp"
"src/collision/ContactManifold.h"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
@ -167,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
"src/collision/CollisionCallback.h"
"src/collision/CollisionCallback.cpp"
"src/collision/OverlapCallback.h"
"src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h"
"src/mathematics/mathematics_functions.cpp"

6
src/body/CollisionBody.cpp
View File

@ -177,7 +177,7 @@ void CollisionBody::resetContactManifoldsList() {
// Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != nullptr) {
ContactManifoldListElement* nextElement = currentElement->next;
ContactManifoldListElement* nextElement = currentElement->getNext();
// Delete the current element
currentElement->~ContactManifoldListElement();
@ -272,8 +272,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
// this body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != nullptr) {
currentElement->contactManifold->mIsAlreadyInIsland = false;
currentElement = currentElement->next;
currentElement->getContactManifold()->mIsAlreadyInIsland = false;
currentElement = currentElement->getNext();
nbManifolds++;
}

79
src/collision/CollisionCallback.cpp Normal file
View File

@ -0,0 +1,79 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "collision/CollisionCallback.h"
#include "engine/OverlappingPair.h"
#include "memory/Allocator.h"
#include "collision/ContactManifold.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) :
contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
body2(pair->getShape2()->getBody()),
proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
mMemoryAllocator(allocator) {
assert(pair != nullptr);
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair
ContactManifold* contactManifold = manifoldSet.getContactManifolds();
while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
contactManifoldElements);
contactManifoldElements = element;
contactManifold = contactManifold->getNext();
}
}
// Destructor
CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
// Release memory allocator for the contact manifold list elements
ContactManifoldListElement* element = contactManifoldElements;
while (element != nullptr) {
ContactManifoldListElement* nextElement = element->getNext();
// Delete and release memory
element->~ContactManifoldListElement();
mMemoryAllocator.release(element, sizeof(ContactManifoldListElement));
element = nextElement;
}
}

89
src/collision/CollisionCallback.h Normal file
View File

@ -0,0 +1,89 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H
#define REACTPHYSICS3D_COLLISION_CALLBACK_H
// Libraries
#include "collision/ContactManifold.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
* the notifyContact() method. This method will called each time a contact
* point is reported.
*/
class CollisionCallback {
public:
// Structure CollisionCallbackInfo
/**
* This structure represents the contact information between two collision
* shapes of two bodies
*/
struct CollisionCallbackInfo {
public:
/// Pointer to the first element of the linked-list that contains contact manifolds
ContactManifoldListElement* contactManifoldElements;
/// Pointer to the first body of the contact
CollisionBody* body1;
/// Pointer to the second body of the contact
CollisionBody* body2;
/// Pointer to the proxy shape of first body
const ProxyShape* proxyShape1;
/// Pointer to the proxy shape of second body
const ProxyShape* proxyShape2;
/// Memory allocator
Allocator& mMemoryAllocator;
// Constructor
CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator);
// Destructor
~CollisionCallbackInfo();
};
/// Destructor
virtual ~CollisionCallback() = default;
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
};
}
#endif

425
src/collision/CollisionDetection.cpp
View File

@ -26,10 +26,13 @@
// Libraries
#include "CollisionDetection.h"
#include "engine/CollisionWorld.h"
#include "collision/OverlapCallback.h"
#include "body/Body.h"
#include "collision/shapes/BoxShape.h"
#include "body/RigidBody.h"
#include "configuration.h"
#include "collision/CollisionCallback.h"
#include "collision/OverlapCallback.h"
#include <cassert>
#include <complex>
#include <set>
@ -43,7 +46,7 @@ using namespace std;
// Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator)
: mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
: mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) {
@ -83,7 +86,7 @@ void CollisionDetection::computeBroadPhase() {
// Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision
// detection.
mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator);
mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator);
}
}
@ -101,25 +104,24 @@ void CollisionDetection::computeMiddlePhase() {
OverlappingPair* pair = it->second;
// Make all the contact manifolds and contact points of the pair obselete
pair->makeContactsObselete();
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the
// Check if the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
@ -128,56 +130,61 @@ void CollisionDetection::computeMiddlePhase() {
++it;
}
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
// Check if the collision filtering allows collision between the two shapes
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) {
// Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
if (!isBody1Active && !isBody2Active) continue;
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
// Check if the bodies are in the set of bodies that cannot collide between each other
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
if (!isBody1Active && !isBody2Active) continue;
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
// Check if the bodies are in the set of bodies that cannot collide between each other
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// If both shapes are convex
if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
shape2->getCachedCollisionData());
mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
// If both shapes are convex
if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
}
// Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
shape2->getCachedCollisionData());
mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
NarrowPhaseInfo* narrowPhaseInfo = nullptr;
computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
// Add all the narrow-phase info object reported by the callback into the
// list of all the narrow-phase info object
while (narrowPhaseInfo != nullptr) {
NarrowPhaseInfo* next = narrowPhaseInfo->next;
narrowPhaseInfo->next = mNarrowPhaseInfoList;
mNarrowPhaseInfoList = narrowPhaseInfo;
narrowPhaseInfo = next;
}
}
// Concave vs Concave shape
else {
// Not handled
continue;
// Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
NarrowPhaseInfo* narrowPhaseInfo = nullptr;
computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
// Add all the narrow-phase info object reported by the callback into the
// list of all the narrow-phase info object
while (narrowPhaseInfo != nullptr) {
NarrowPhaseInfo* next = narrowPhaseInfo->next;
narrowPhaseInfo->next = mNarrowPhaseInfoList;
mNarrowPhaseInfoList = narrowPhaseInfo;
narrowPhaseInfo = next;
}
}
// Concave vs Concave shape
else {
// Not handled
continue;
}
}
}
}
@ -231,7 +238,7 @@ void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()");
const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
while (currentNarrowPhaseInfo != nullptr) {
// Select the narrow phase algorithm to use according to the two collision shapes
@ -245,30 +252,16 @@ void CollisionDetection::computeNarrowPhase() {
// 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.
ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform);
// If it is the first contact since the pairs are overlapping
if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo);
}
// Add the contact manifold to the corresponding overlapping pair
currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo);
// Add the contact points as a potential contact manifold into the pair
currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
// Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
currentNarrowPhaseInfo->overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
// Trigger a callback event for the new contact
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
}
else {
@ -282,8 +275,14 @@ void CollisionDetection::computeNarrowPhase() {
currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
}
// Convert the potential contact into actual contacts
processAllPotentialContacts();
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
// Report contacts to the user
reportAllContacts();
}
// Allow the broadphase to notify the collision detection about an overlapping pair.
@ -302,13 +301,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
// Check if the overlapping pair already exists
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// Compute the maximum number of contact manifolds for this pair
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
shape2->getCollisionShape()->getType());
// Create the overlapping pair and add it into the set of overlapping pairs
OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator);
OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator);
assert(newPair != nullptr);
#ifndef NDEBUG
@ -372,40 +367,173 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
ContactManifold* contactManifold = manifoldSet.getContactManifolds();
while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
contactManifold = contactManifold->getNext();
}
}
// Delete all the contact points in the currently overlapping pairs
void CollisionDetection::clearContactPoints() {
/// Convert the potential contact into actual contacts
void CollisionDetection::processAllPotentialContacts() {
// For each overlapping pair
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
it->second->clearContactPoints();
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// Process the potential contacts of the overlapping pair
processPotentialContacts(it->second);
}
}
// Process the potential contact manifold of a pair to create actual contact manifold
void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
// Reduce the number of contact points of the manifold
pair->reducePotentialContactManifolds();
// If there is a concave mesh shape in the pair
if (pair->hasConcaveShape()) {
processSmoothMeshContacts(pair);
}
else { // If both collision shapes are convex
// Add all the potential contact manifolds as actual contact manifolds to the pair
ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
while (potentialManifold != nullptr) {
pair->addContactManifold(potentialManifold);
potentialManifold = potentialManifold->mNext;
}
}
// Clear the obselete contact manifolds and contact points
pair->clearObseleteManifoldsAndContactPoints();
// Reset the potential contacts of the pair
pair->clearPotentialContactManifolds();
}
// Report contacts for all the colliding overlapping pairs
void CollisionDetection::reportAllContacts() {
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// If there is a user callback
if (mWorld->mEventListener != nullptr) {
CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
// Trigger a callback event to report the new contact to the user
mWorld->mEventListener->newContact(collisionInfo);
}
}
}
// Process the potential contacts where one collion is a concave shape.
// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges.
void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) {
// // Set with the triangle vertices already processed to void further contacts with same triangle
// std::unordered_multimap<int, Vector3> processTriangleVertices;
// std::vector<SmoothMeshContactInfo*> smoothContactPoints;
// // If the collision shape 1 is the triangle
// bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE;
// assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE);
// assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE);
// const TriangleShape* triangleShape = nullptr;
// if (isFirstShapeTriangle) {
// triangleShape = static_cast<const TriangleShape*>(pair->getShape1()->getCollisionShape());
// }
// else {
// triangleShape = static_cast<const TriangleShape*>(pair->getShape2()->getCollisionShape());
// }
// assert(triangleShape != nullptr);
// // Get the temporary memory allocator
// Allocator& allocator = pair->getTemporaryAllocator();
// // For each potential contact manifold of the pair
// ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
// while (potentialManifold != nullptr) {
// // For each contact point of the potential manifold
// ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo();
// while (contactPointInfo != nullptr) {
// // Compute the barycentric coordinates of the point in the triangle
// decimal u, v, w;
// computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0),
// triangleShape->getVertexPosition(1),
// triangleShape->getVertexPosition(2),
// isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2,
// u, v, w);
// int nbZeros = 0;
// bool isUZero = approxEqual(u, 0, 0.0001);
// bool isVZero = approxEqual(v, 0, 0.0001);
// bool isWZero = approxEqual(w, 0, 0.0001);
// if (isUZero) nbZeros++;
// if (isVZero) nbZeros++;
// if (isWZero) nbZeros++;
// // If the triangle contact point is on a triangle vertex of a triangle edge
// if (nbZeros == 1 || nbZeros == 2) {
// // Create a smooth mesh contact info
// SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo)))
// SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle,
// triangleShape->getVertexPosition(0),
// triangleShape->getVertexPosition(1),
// triangleShape->getVertexPosition(2));
// smoothContactPoints.push_back(smoothContactInfo);
// // Remove the contact point info from the manifold. If the contact point will be kept in the future, we
// // will put the contact point back in the manifold.
// ...
// }
// // Note that we do not remove the contact points that are not on the vertices or edges of the triangle
// // from the contact manifold because we know we will keep to contact points. We only remove the vertices
// // and edges contact points for the moment. If those points will be kept in the future, we will have to
// // put them back again in the contact manifold
// }
// potentialManifold = potentialManifold->mNext;
// }
// // Sort the list of narrow-phase contacts according to their penetration depth
// std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare());
// ...
}
// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) {
@ -424,7 +552,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
shape2->getCachedCollisionData());
@ -436,7 +564,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// 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, mMemoryAllocator, &narrowPhaseInfo);
computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo);
}
return narrowPhaseInfo;
@ -450,7 +578,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
std::unordered_set<bodyindex> reportedBodies;
// Ask the broad-phase to get all the overlapping shapes
LinkedList<int> overlappingNodes(mMemoryAllocator);
LinkedList<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
// For each overlaping proxy shape
@ -504,7 +632,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
// Create a temporary overlapping pair
OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@ -526,16 +654,18 @@ 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.
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
}
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
// Call the destructor
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision
@ -570,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
// Ask the broad-phase to get all the overlapping shapes
LinkedList<int> overlappingNodes(mMemoryAllocator);
LinkedList<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
const bodyindex bodyId = body->getID();
@ -595,7 +725,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
// Create a temporary overlapping pair
OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@ -617,16 +747,18 @@ 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.
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
}
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
// Call the destructor
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision
@ -673,7 +805,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
if (aabb1.testCollision(aabb2)) {
// Create a temporary overlapping pair
OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@ -693,26 +825,29 @@ 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.
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
body1ProxyShape, body2ProxyShape);
// Report the contact to the user
collisionCallback->notifyContact(collisionInfo);
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
}
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
// Call the destructor
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Process the potential contacts
processPotentialContacts(&pair);
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
collisionCallback->notifyContact(collisionInfo);
}
// Go to the next proxy shape
@ -737,7 +872,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
// Ask the broad-phase to get all the overlapping shapes
LinkedList<int> overlappingNodes(mMemoryAllocator);
LinkedList<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
const bodyindex bodyId = body->getID();
@ -760,7 +895,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
// Create a temporary overlapping pair
OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@ -777,17 +912,13 @@ 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.
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
proxyShape->getBody(), bodyProxyShape,
proxyShape);
// Report the contact to the user
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
}
@ -795,9 +926,15 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
// Call the destructor
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Process the potential contacts
processPotentialContacts(&pair);
}
}
@ -822,10 +959,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;
OverlappingPair* originalPair = it->second;
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
// Create a new overlapping pair so that we do not work on the original one
OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator,
mPoolAllocator);
ProxyShape* shape1 = pair.getShape1();
ProxyShape* shape2 = pair.getShape2();
// Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping.
@ -834,14 +975,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair);
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
// For each narrow-phase info object
while (narrowPhaseInfo != nullptr) {
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@ -851,29 +992,29 @@ 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.
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
// Reduce the number of points in the contact manifold (if necessary)
contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
shape2->getBody(), shape1, shape2);
// Report the contact to the user
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
// The previous frame collision info is now valid
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
// Call the destructor
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Process the potential contacts
processPotentialContacts(&pair);
}
}
}

17
src/collision/CollisionDetection.h
View File

@ -72,7 +72,7 @@ class CollisionDetection {
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
/// Reference to the memory allocator
PoolAllocator& mMemoryAllocator;
PoolAllocator& mPoolAllocator;
/// Reference to the single frame memory allocator
SingleFrameAllocator& mSingleFrameAllocator;
@ -118,9 +118,6 @@ class CollisionDetection {
/// involed in the corresponding contact.
void addContactManifoldToBody(OverlappingPair* pair);
/// Delete all the contact points in the currently overlapping pairs
void clearContactPoints();
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
@ -137,6 +134,18 @@ class CollisionDetection {
/// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair);
/// Convert the potential contact into actual contacts
void processAllPotentialContacts();
/// Process the potential contact manifold of a pair to create actual contact manifold
void processPotentialContacts(OverlappingPair* pair);
/// Report contacts for all the colliding overlapping pairs
void reportAllContacts();
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
public :

82
src/collision/ContactManifold.cpp
View File

@ -30,15 +30,14 @@
using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo();
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
while(pointInfo != nullptr) {
// Create the new contact point
@ -46,7 +45,8 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS
ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
// Add the new contact point into the manifold
mContactPoints[mNbContactPoints] = contact;
contact->setNext(mContactPoints);
mContactPoints = contact;
mNbContactPoints++;
pointInfo = pointInfo->next;
@ -58,49 +58,69 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS
// Destructor
ContactManifold::~ContactManifold() {
clear();
}
// Clear the contact manifold
void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) {
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
mContactPoints[i]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
// Delete all the contact points
ContactPoint* contactPoint = mContactPoints;
while(contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
// TODO : Delete this
bool test = mMemoryAllocator.isReleaseNeeded();
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
contactPoint = nextContactPoint;
}
mNbContactPoints = 0;
}
// Add a contact point
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD);
// Create the new contact point
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
// Add the new contact point into the manifold
mContactPoints[mNbContactPoints] = contactPoint;
mNbContactPoints++;
contactPoint->setNext(mContactPoints);
mContactPoints = contactPoint;
mNbContactPoints++;
}
// Remove a contact point
void ContactManifold::removeContactPoint(int index) {
// Clear the obselete contact points
void ContactManifold::clearObseleteContactPoints() {
assert(mNbContactPoints > 0);
assert(index >= 0 && index < mNbContactPoints);
ContactPoint* contactPoint = mContactPoints;
ContactPoint* previousContactPoint = nullptr;
while (contactPoint != nullptr) {
// Delete the contact
mContactPoints[index]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
ContactPoint* nextContactPoint = contactPoint->getNext();
for (int i=index; (i+1) < mNbContactPoints; i++) {
mContactPoints[i] = mContactPoints[i+1];
if (contactPoint->getIsObselete()) {
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
if (previousContactPoint != nullptr) {
previousContactPoint->setNext(nextContactPoint);
}
else {
mContactPoints = nextContactPoint;
}
mNbContactPoints--;
}
else {
previousContactPoint = contactPoint;
}
contactPoint = nextContactPoint;
}
mNbContactPoints--;
assert(mNbContactPoints >= 0);
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
}

248
src/collision/ContactManifold.h
View File

@ -46,40 +46,43 @@ class ContactManifold;
*/
struct ContactManifoldListElement {
public:
private:
// -------------------- Attributes -------------------- //
/// Pointer to the actual contact manifold
ContactManifold* contactManifold;
/// Pointer to a contact manifold with contact points
ContactManifold* mContactManifold;
/// Next element of the list
ContactManifoldListElement* next;
ContactManifoldListElement* mNext;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldListElement(ContactManifold* initContactManifold,
ContactManifoldListElement* initNext)
:contactManifold(initContactManifold), next(initNext) {
ContactManifoldListElement(ContactManifold* contactManifold,
ContactManifoldListElement* next)
:mContactManifold(contactManifold), mNext(next) {
}
/// Return the contact manifold
ContactManifold* getContactManifold() {
return mContactManifold;
}
/// Return the next element in the linked-list
ContactManifoldListElement* getNext() {
return mNext;
}
};
// Class ContactManifold
/**
* This class represents the set of contact points between two bodies.
* The contact manifold is implemented in a way to cache the contact
* points among the frames for better stability following the
* "Contact Generation" presentation of Erwin Coumans at GDC 2010
* conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
* Some code of this class is based on the implementation of the
* btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
* The contacts between two bodies are added one after the other in the cache.
* When the cache is full, we have to remove one point. The idea is to keep
* the point with the deepest penetration depth and also to keep the
* points producing the larger area (for a more stable contact manifold).
* The new added point is always kept.
* points among the frames for better stability.
*/
class ContactManifold {
@ -94,10 +97,10 @@ class ContactManifold {
ProxyShape* mShape2;
/// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
ContactPoint* mContactPoints;
/// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId;
short int mContactNormalId;
/// Number of contacts in the cache
int8 mNbContactPoints;
@ -124,20 +127,86 @@ class ContactManifold {
bool mIsAlreadyInIsland;
/// Reference to the memory allocator
PoolAllocator& mMemoryAllocator;
Allocator& mMemoryAllocator;
/// Pointer to the next contact manifold in the linked-list
ContactManifold* mNext;
/// Pointer to the previous contact manifold in linked-list
ContactManifold* mPrevious;
/// True if the contact manifold is obselete
bool mIsObselete;
// -------------------- Methods -------------------- //
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
/// Set the pointer to the next element in the linked-list
void setNext(ContactManifold* nextManifold);
/// Return true if the manifold is obselete
bool getIsObselete() const;
/// Set to true to make the manifold obselete
void setIsObselete(bool isObselete, bool setContactPoints);
/// Clear the obselete contact points
void clearObseleteContactPoints();
/// Return the contact normal direction Id of the manifold
short getContactNormalId() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1);
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
/// Add a contact point
void addContactPoint(const ContactPointInfo* contactPointInfo);
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Set the pointer to the previous element in the linked-list
void setPrevious(ContactManifold* previousManifold);
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, short int normalDirectionId);
ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
Allocator& memoryAllocator);
/// Destructor
~ContactManifold();
@ -160,68 +229,25 @@ class ContactManifold {
/// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const;
/// Return the normal direction Id
short int getNormalDirectionId() const;
/// Clear the contact manifold
void clear();
/// Return the number of contact points in the manifold
int8 getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
/// Return a pointer to the first contact point of the manifold
ContactPoint* getContactPoints() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1);
/// Return a pointer to the previous element in the linked-list
ContactManifold* getPrevious() const;
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const;
/// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// Add a contact point
void addContactPoint(const ContactPointInfo* contactPointInfo);
/// Remove a contact point
void removeContactPoint(int index);
/// Return a pointer to the next element in the linked-list
ContactManifold* getNext() const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class Island;
friend class CollisionBody;
friend class ContactManifoldSet;
friend class ContactSolver;
};
// Return a pointer to the first proxy shape of the contact
@ -244,11 +270,6 @@ inline CollisionBody* ContactManifold::getBody2() const {
return mShape2->getBody();
}
// Return the normal direction Id
inline short int ContactManifold::getNormalDirectionId() const {
return mNormalDirectionId;
}
// Return the number of contact points in the manifold
inline int8 ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
@ -309,10 +330,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
mRollingResistanceImpulse = rollingResistanceImpulse;
}
// Return a contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
assert(index < mNbContactPoints);
return mContactPoints[index];
// Return a pointer to the first contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoints() const {
return mContactPoints;
}
// Return true if the contact manifold has already been added into an island
@ -320,31 +340,69 @@ inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
// Return the normalized averaged normal vector
inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal;
for (uint i=0; i<mNbContactPoints; i++) {
averageNormal += mContactPoints[i]->getNormal();
}
return averageNormal.getUnit();
}
// Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
for (uint i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth();
assert(mNbContactPoints > 0);
ContactPoint* contactPoint = mContactPoints;
while(contactPoint != nullptr){
decimal depth = contactPoint->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
contactPoint = contactPoint->getNext();
}
return largestDepth;
}
// Return a pointer to the previous element in the linked-list
inline ContactManifold* ContactManifold::getPrevious() const {
return mPrevious;
}
// Set the pointer to the previous element in the linked-list
inline void ContactManifold::setPrevious(ContactManifold* previousManifold) {
mPrevious = previousManifold;
}
// Return a pointer to the next element in the linked-list
inline ContactManifold* ContactManifold::getNext() const {
return mNext;
}
// Set the pointer to the next element in the linked-list
inline void ContactManifold::setNext(ContactManifold* nextManifold) {
mNext = nextManifold;
}
// Return true if the manifold is obselete
inline bool ContactManifold::getIsObselete() const {
return mIsObselete;
}
// Set to true to make the manifold obselete
inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) {
mIsObselete = isObselete;
if (setContactPoints) {
ContactPoint* contactPoint = mContactPoints;
while (contactPoint != nullptr) {
contactPoint->setIsObselete(isObselete);
contactPoint = contactPoint->getNext();
}
}
}
// Return the contact normal direction Id of the manifold
inline short ContactManifold::getContactNormalId() const {
return mContactNormalId;
}
}
#endif

60
src/collision/ContactManifoldInfo.cpp
View File

@ -30,7 +30,8 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
: mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {}
: mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator),
mContactNormalId(-1) {}
// Destructor
ContactManifoldInfo::~ContactManifoldInfo() {
@ -40,14 +41,13 @@ ContactManifoldInfo::~ContactManifoldInfo() {
}
// Add a new contact point into the manifold
void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) {
assert(penDepth > decimal(0.0));
assert(contactPointInfo->penetrationDepth > decimal(0.0));
assert(contactNormalId >= 0);
assert(mContactNormalId == -1 || contactNormalId == mContactNormalId);
// Create the contact point info
ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
mContactNormalId = contactNormalId;
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
@ -73,15 +73,31 @@ void ContactManifoldInfo::reset() {
mNbContactPoints = 0;
}
// Reduce the number of points in the contact manifold
// Return the largest penetration depth among its contact points
decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
ContactPointInfo* contactPoint = mContactPointsList;
assert(contactPoint != nullptr);
decimal maxDepth = decimal(0.0);
while (contactPoint != nullptr) {
if (contactPoint->penetrationDepth > maxDepth) {
maxDepth = contactPoint->penetrationDepth;
}
contactPoint = contactPoint->next;
}
return maxDepth;
}
// Reduce the number of contact points of the currently computed manifold
// This is based on the technique described by Dirk Gregorius in his
// "Contacts Creation" GDC presentation
void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
assert(mContactPointsList != nullptr);
// TODO : Implement this (do not forget to deallocate removed points)
// The following algorithm only works to reduce to 4 contact points
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
@ -221,9 +237,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
largestArea = area;
pointsToKeep[3] = element;
}
element = element->next;
}
element = element->next;
}
assert(pointsToKeep[3] != nullptr);
@ -250,6 +266,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
}
element = element->next;
// Call the destructor
elementToDelete->~ContactPointInfo();
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
@ -258,21 +277,4 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
}
}
/// Return the largest penetration depth among the contact points
decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
decimal maxDepth = decimal(0.0);
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
if (element->penetrationDepth > maxDepth) {
maxDepth = element->penetrationDepth;
}
element = element->next;
}
return maxDepth;
}

45
src/collision/ContactManifoldInfo.h
View File

@ -34,7 +34,7 @@
namespace reactphysics3d {
// Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class ContactManifoldInfo
/**
@ -50,12 +50,18 @@ class ContactManifoldInfo {
/// Linked list with all the contact points
ContactPointInfo* mContactPointsList;
/// Memory allocator used to allocate contact points
Allocator& mAllocator;
/// Number of contact points in the manifold
uint mNbContactPoints;
/// Next element in the linked-list of contact manifold info
ContactManifoldInfo* mNext;
/// Reference the the memory allocator where the contact point infos have been allocated
Allocator& mAllocator;
/// Contact normal direction Id (Identify the contact normal direction of points in manifold)
short mContactNormalId;
public:
// -------------------- Methods -------------------- //
@ -66,15 +72,14 @@ class ContactManifoldInfo {
/// Destructor
~ContactManifoldInfo();
/// Deleted copy-constructor
/// Deleted Copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
/// Deleted assignment operator
ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
/// Add a new contact point into the manifold
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2);
void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId);
/// Remove all the contact points
void reset();
@ -82,11 +87,21 @@ class ContactManifoldInfo {
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const;
/// Reduce the number of points in the contact manifold
/// Return the largest penetration depth among its contact points
decimal getLargestPenetrationDepth() const;
/// Return the pointer to the next manifold info in the linked-list
ContactManifoldInfo* getNext();
/// Return the contact normal Id
short getContactNormalId() const;
/// Reduce the number of contact points of the currently computed manifold
void reduce(const Transform& shape1ToWorldTransform);
/// Return the largest penetration depth among the contact points
decimal getLargestPenetrationDepth() const;
// Friendship
friend class OverlappingPair;
friend class CollisionDetection;
};
// Get the first contact point info of the linked list of contact points
@ -94,6 +109,16 @@ inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
return mContactPointsList;
}
// Return the pointer to the next manifold info in the linked-list
inline ContactManifoldInfo* ContactManifoldInfo::getNext() {
return mNext;
}
// Return the contact normal Id
inline short ContactManifoldInfo::getContactNormalId() const {
return mContactNormalId;
}
}
#endif

278
src/collision/ContactManifoldSet.cpp
View File

@ -30,10 +30,12 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, int nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) {
Allocator& memoryAllocator)
: mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) {
// Compute the maximum number of manifolds allowed between the two shapes
mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
}
// Destructor
@ -43,167 +45,131 @@ ContactManifoldSet::~ContactManifoldSet() {
clear();
}
void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr);
assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr);
// If there is no contact manifold yet
if (mNbManifolds == 0) {
// Try to find an existing contact manifold with similar contact normal
ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId());
// If the maximum number of manifold is 1
if (mNbMaxManifolds == 1) {
createManifold(contactManifoldInfo, 0);
}
else {
// If a similar manifold has been found
if (similarManifold != nullptr) {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
createManifold(contactManifoldInfo, normalDirectionId);
}
// Update the old manifold with the new one
updateManifoldWithNewOne(similarManifold, contactManifoldInfo);
}
else { // If there is already at least one contact manifold in the set
else {
// If the maximum number of manifold is 1
if (mNbMaxManifolds == 1) {
// If there are too much contact manifolds in the set
if (mNbManifolds >= mNbMaxManifolds) {
// Replace the old manifold with the new one
updateManifoldWithNewOne(0, contactManifoldInfo);
}
else {
// We need to remove a manifold from the set.
// We seach for the one with the smallest maximum penetration depth among its contact points
ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth());
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
// If the manifold with the minimum penetration depth is an existing one
if (minDepthManifold != nullptr) {
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
}
// Remove the manifold
removeManifold(minDepthManifold);
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
// Replace the old manifold with the new one
updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo);
}
else {
// If we have not reach the maximum number of manifolds
if (mNbManifolds < mNbMaxManifolds) {
// Create the new contact manifold
createManifold(contactManifoldInfo, normalDirectionId);
}
else {
decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth();
// We have reached the maximum number of manifold, we do not
// want to keep the manifold with the smallest penetration detph
int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth);
// If the manifold with the smallest penetration depth is not the new one,
// we have to keep the new manifold and remove the one with the smallest depth
if (smallestPenDepthManifoldIndex >= 0) {
removeManifold(smallestPenDepthManifoldIndex);
createManifold(contactManifoldInfo, normalDirectionId);
}
}
// Create a new contact manifold
createManifold(contactManifoldInfo);
}
}
// Create a new contact manifold
createManifold(contactManifoldInfo);
}
}
// Update a previous similar manifold with a new one
void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) {
void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
// For each contact point of the previous manifold
for (int i=0; i<mManifolds[oldManifoldIndex]->getNbContactPoints(); ) {
assert(oldManifold != nullptr);
assert(newManifold != nullptr);
ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i);
// For each contact point of the new manifold
ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
assert(contactPointInfo != nullptr);
while (contactPointInfo != nullptr) {
// For each contact point in the new manifold
ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo();
bool needToRemovePoint = true;
while (newPoint != nullptr) {
// For each contact point in the old manifold
bool isSimilarPointFound = false;
ContactPoint* oldContactPoint = oldManifold->getContactPoints();
while (oldContactPoint != nullptr) {
assert(oldContactPoint != nullptr);
// If the new contact point is similar (very close) to the old contact point
if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) {
if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
// Replace (update) the old contact point with the new one
contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(),
mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform());
needToRemovePoint = false;
newPoint->isUsed = true;
oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
isSimilarPointFound = true;
break;
}
newPoint = newPoint->next;
oldContactPoint = oldContactPoint->getNext();
}
// If no new contact point is similar to the old one
if (needToRemovePoint) {
// If we have not found a similar contact point
if (!isSimilarPointFound) {
// Remove the old contact point
mManifolds[oldManifoldIndex]->removeContactPoint(i);
}
else {
i++;
// Add the contact point to the manifold
oldManifold->addContactPoint(contactPointInfo);
}
contactPointInfo = contactPointInfo->next;
}
// For each point of the new manifold that have not been used yet (to update
// an existing point in the previous manifold), add it into the previous manifold
const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo();
while (newPointInfo != nullptr) {
if (!newPointInfo->isUsed) {
mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo);
}
newPointInfo = newPointInfo->next;
}
// The old manifold is no longer obselete
oldManifold->setIsObselete(false, false);
}
// Return the manifold with the smallest contact penetration depth
int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
// Return the manifold with the smallest contact penetration depth among its points
ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
// The contact point will be in a new contact manifold, we now have too much
// manifolds condidates. We need to remove one. We choose to remove the manifold
// with the smallest contact depth among their points
int smallestDepthManifoldIndex = -1;
decimal minDepth = initDepth;
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
ContactManifold* minDepthManifold = nullptr;
decimal minDepth = initDepth;
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
decimal depth = manifold->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
smallestDepthManifoldIndex = i;
minDepthManifold = manifold;
}
manifold = manifold->getNext();
}
return smallestDepthManifoldIndex;
return minDepthManifold;
}
// Return the index of the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns -1
int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
// Return the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns nullptr
ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
ContactManifold* manifold = mManifolds;
// Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<mNbManifolds; i++) {
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i;
while (manifold != nullptr) {
if (normalDirectionId == manifold->getContactNormalId()) {
return manifold;
}
manifold = manifold->getNext();
}
return -1;
return nullptr;
}
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) {
assert(normal.lengthSquare() > MACHINE_EPSILON);
@ -240,36 +206,100 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
// Clear the contact manifold set
void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) {
removeManifold(i);
ContactManifold* manifold = mManifolds;
while(manifold != nullptr) {
ContactManifold* nextManifold = manifold->getNext();
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
manifold = nextManifold;
mNbManifolds--;
}
assert(mNbManifolds == 0);
}
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) {
void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
assert(mNbManifolds < mNbMaxManifolds);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId);
ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
manifold->setPrevious(nullptr);
manifold->setNext(mManifolds);
mManifolds = manifold;
mNbManifolds++;
}
// Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int index) {
void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds);
// Delete the new contact
mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
ContactManifold* previous = manifold->getPrevious();
ContactManifold* next = manifold->getNext();
for (int i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1];
if (previous != nullptr) {
previous->setNext(manifold->getNext());
}
if (next != nullptr) {
next->setPrevious(manifold->getPrevious());
}
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
mNbManifolds--;
}
// Make all the contact manifolds and contact points obselete
void ContactManifoldSet::makeContactsObselete() {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
manifold->setIsObselete(true, true);
manifold = manifold->getNext();
}
}
// Clear the obselete contact manifolds and contact points
void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() {
ContactManifold* manifold = mManifolds;
ContactManifold* previousManifold = nullptr;
while (manifold != nullptr) {
ContactManifold* nextManifold = manifold->getNext();
if (manifold->getIsObselete()) {
if (previousManifold != nullptr) {
previousManifold->setNext(nextManifold);
if (nextManifold != nullptr) {
nextManifold->setPrevious(previousManifold);
}
}
else {
mManifolds = nextManifold;
}
// Delete the contact manifold
removeManifold(manifold);
}
else {
manifold->clearObseleteContactPoints();
previousManifold = manifold;
}
manifold = nextManifold;
}
}

84
src/collision/ContactManifoldSet.h
View File

@ -60,33 +60,34 @@ class ContactManifoldSet {
/// Pointer to the second proxy shape of the contact
ProxyShape* mShape2;
/// Reference to the memory allocator
PoolAllocator& mMemoryAllocator;
/// Reference to the memory allocator for the contact manifolds
Allocator& mMemoryAllocator;
/// Contact manifolds of the set
ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
ContactManifold* mManifolds;
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId);
void createManifold(const ContactManifoldInfo* manifoldInfo);
/// Remove a contact manifold from the set
void removeManifold(int index);
// Return the index of the contact manifold with a similar average normal.
int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const;
// Return the contact manifold with a similar average normal.
ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
/// Return the manifold with the smallest contact penetration depth
int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
/// Update a previous similar manifold with a new one
void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold);
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
/// Return the maximum number of contact manifolds allowed between to collision shapes
int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2);
/// Clear the contact manifold set
void clear();
/// Delete a contact manifold
void removeManifold(ContactManifold* manifold);
public:
@ -94,13 +95,13 @@ class ContactManifoldSet {
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, int nbMaxManifolds);
Allocator& memoryAllocator);
/// Destructor
~ContactManifoldSet();
/// Add a contact manifold in the set
void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
/// Return the first proxy shape
ProxyShape* getShape1() const;
@ -108,17 +109,25 @@ class ContactManifoldSet {
/// Return the second proxy shape
ProxyShape* getShape2() const;
/// Clear the contact manifold set
void clear();
/// Return the number of manifolds in the set
int getNbContactManifolds() const;
/// Return a given contact manifold
ContactManifold* getContactManifold(int index) const;
/// Return a pointer to the first element of the linked-list of contact manifolds
ContactManifold* getContactManifolds() const;
/// Make all the contact manifolds and contact points obselete
void makeContactsObselete();
/// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const;
/// Clear the obselete contact manifolds and contact points
void clearObseleteManifoldsAndContactPoints();
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
static short int computeCubemapNormalId(const Vector3& normal);
};
// Return the first proxy shape
@ -136,21 +145,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds;
}
// Return a given contact manifold
inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const {
assert(index >= 0 && index < mNbManifolds);
return mManifolds[index];
// Return a pointer to the first element of the linked-list of contact manifolds
inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
return mManifolds;
}
// Return the total number of contact points in the set of manifolds
inline int ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0;
for (int i=0; i<mNbManifolds; i++) {
nbPoints += mManifolds[i]->getNbContactPoints();
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
nbPoints += manifold->getNbContactPoints();
manifold = manifold->getNext();
}
return nbPoints;
}
// Return the maximum number of contact manifolds allowed between to collision shapes
inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
// If both shapes are convex
if (shape1->isConvex() && shape2->isConvex()) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape
else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
}
}
}
#endif

97
src/collision/NarrowPhaseInfo.cpp Normal file
View File

@ -0,0 +1,97 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include <iostream>
#include "NarrowPhaseInfo.h"
#include "ContactPointInfo.h"
#include "engine/OverlappingPair.h"
using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
const CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, void** cachedData1, void** cachedData2)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(nullptr), cachedCollisionData1(cachedData1),
cachedCollisionData2(cachedData2), next(nullptr) {
}
// Destructor
NarrowPhaseInfo::~NarrowPhaseInfo() {
assert(contactPoints == nullptr);
}
// Add a new contact point
void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Get the memory allocator
Allocator& allocator = overlappingPair->getTemporaryAllocator();
// Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the linked list of contact points
contactPointInfo->next = contactPoints;
contactPoints = contactPointInfo;
}
/// Take all the generated contact points and create a new potential
/// contact manifold into the overlapping pair
void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() {
overlappingPair->addPotentialContactPoints(this);
}
// Reset the remaining contact points
void NarrowPhaseInfo::resetContactPoints() {
// Get the memory allocator
Allocator& allocator = overlappingPair->getTemporaryAllocator();
// For each remaining contact point info
ContactPointInfo* element = contactPoints;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Call the destructor
elementToDelete->~ContactPointInfo();
// Delete the current element
allocator.release(elementToDelete, sizeof(ContactPointInfo));
}
contactPoints = nullptr;
}

22
src/collision/NarrowPhaseInfo.h
View File

@ -28,6 +28,7 @@
// Libraries
#include "shapes/CollisionShape.h"
#include "collision/ContactManifoldInfo.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -58,6 +59,9 @@ struct NarrowPhaseInfo {
/// Transform that maps from collision shape 2 local-space to world-space
Transform shape2ToWorldTransform;
/// Linked-list of contact points created during the narrow-phase
ContactPointInfo* contactPoints;
/// Cached collision data of the proxy shape
// TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
void** cachedCollisionData1;
@ -72,12 +76,20 @@ struct NarrowPhaseInfo {
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
const CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, void** cachedData1, void** cachedData2)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) {
const Transform& shape2Transform, void** cachedData1, void** cachedData2);
}
/// Destructor
~NarrowPhaseInfo();
/// Add a new contact point
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2);
/// Create a new potential contact manifold into the overlapping pair using current contact points
void addContactPointsAsPotentialContactManifold();
/// Reset the remaining contact points
void resetContactPoints();
};
}

57
src/collision/OverlapCallback.h Normal file
View File

@ -0,0 +1,57 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H
#define REACTPHYSICS3D_OVERLAP_CALLBACK_H
// Libraries
#include "body/CollisionBody.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class OverlapCallback
/**
* This class can be used to register a callback for collision overlap queries.
* You should implement your own class inherited from this one and implement
* the notifyOverlap() method. This method will called each time a contact
* point is reported.
*/
class OverlapCallback {
public:
/// Destructor
virtual ~OverlapCallback() {
}
/// This method will be called for each reported overlapping bodies
virtual void notifyOverlap(CollisionBody* collisionBody)=0;
};
}
#endif

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

@ -33,7 +33,7 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
@ -116,8 +116,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - segmentsDistance;
// Create the contact info object
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
return true;
}
@ -148,7 +148,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
return true;
}

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

@ -61,8 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}

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

@ -37,13 +37,12 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
SATAlgorithm satAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
@ -61,8 +60,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK
ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo();
assert(contactPoint != nullptr);
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
assert(contactPoint != nullptr);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
@ -96,7 +95,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
contactManifoldInfo.reset();
narrowPhaseInfo->resetContactPoints();
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@ -116,7 +115,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
contactManifoldInfo, isCapsuleShape1);
narrowPhaseInfo, isCapsuleShape1);
break;
}
@ -133,7 +132,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;

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

@ -61,8 +61,7 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}

178
src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -141,114 +141,114 @@ void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges.
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
// std::vector<SmoothMeshContactInfo> contactPoints,
// NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int, Vector3> processTriangleVertices;
// // Set with the triangle vertices already processed to void further contacts with same triangle
// std::unordered_multimap<int, Vector3> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
// // Sort the list of narrow-phase contacts according to their penetration depth
// std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
// For each contact point (from smaller penetration depth to larger)
std::vector<SmoothMeshContactInfo>::const_iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
// // For each contact point (from smaller penetration depth to larger)
// std::vector<SmoothMeshContactInfo>::const_iterator it;
// for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it;
const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// const SmoothMeshContactInfo info = *it;
// const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle
decimal u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
info.triangleVertices[1],
info.triangleVertices[2],
contactPoint, u, v, w);
int nbZeros = 0;
bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 0, 0.0001);
bool isWZero = approxEqual(w, 0, 0.0001);
if (isUZero) nbZeros++;
if (isVZero) nbZeros++;
if (isWZero) nbZeros++;
// // Compute the barycentric coordinates of the point in the triangle
// decimal u, v, w;
// computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
// info.triangleVertices[1],
// info.triangleVertices[2],
// contactPoint, u, v, w);
// int nbZeros = 0;
// bool isUZero = approxEqual(u, 0, 0.0001);
// bool isVZero = approxEqual(v, 0, 0.0001);
// bool isWZero = approxEqual(w, 0, 0.0001);
// if (isUZero) nbZeros++;
// if (isVZero) nbZeros++;
// if (isWZero) nbZeros++;
// If it is a vertex contact
if (nbZeros == 2) {
// // If it is a vertex contact
// if (nbZeros == 2) {
Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// // Check that this triangle vertex has not been processed yet
// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
}
}
else if (nbZeros == 1) { // If it is an edge contact
// // Keep the contact as it is and report it
// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
// }
// }
// else if (nbZeros == 1) { // If it is an edge contact
Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
// Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// // Check that this triangle edge has not been processed yet
// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
// !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
}
// // Keep the contact as it is and report it
// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
// }
}
else { // If it is a face contact
// }
// else { // If it is a face contact
ContactPointInfo newContactInfo(info.contactInfo);
// ContactPointInfo newContactInfo(info.contactInfo);
ProxyShape* firstShape;
ProxyShape* secondShape;
if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1();
secondShape = overlappingPair->getShape2();
}
else {
firstShape = overlappingPair->getShape2();
secondShape = overlappingPair->getShape1();
}
// ProxyShape* firstShape;
// ProxyShape* secondShape;
// if (info.isFirstShapeTriangle) {
// firstShape = overlappingPair->getShape1();
// secondShape = overlappingPair->getShape2();
// }
// else {
// firstShape = overlappingPair->getShape2();
// secondShape = overlappingPair->getShape1();
// }
// We use the triangle normal as the contact normal
Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
Vector3 localNormal = a.cross(b);
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
newContactInfo.normal.normalize();
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal;
}
// // We use the triangle normal as the contact normal
// Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
// Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
// Vector3 localNormal = a.cross(b);
// newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
// Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
// newContactInfo.normal.normalize();
// if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
// newContactInfo.normal = -newContactInfo.normal;
// }
// We recompute the contact point on the second body with the new normal as described in
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque
Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
if (info.isFirstShapeTriangle) {
Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
}
else {
Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
}
// // We recompute the contact point on the second body with the new normal as described in
// // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// // Dirk Gregorius) to avoid adding torque
// Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
// if (info.isFirstShapeTriangle) {
// Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
// newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
// }
// else {
// Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
// newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
// }
// Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
}
// // Report the contact
// narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
// }
// Add the three vertices of the triangle to the set of processed
// triangle vertices
addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
}
}
// // Add the three vertices of the triangle to the set of processed
// // triangle vertices
// addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
// addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
// addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
// }
//}
// Return true if the vertex is in the set of already processed vertices
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {

26
src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
View File

@ -83,25 +83,35 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
// Class SmoothMeshContactInfo
/**
* This class is used to store data about a contact with a triangle for the smooth
* mesh algorithm.
* Contains data for of potential smooth contact during the smooth mesh
* contacts computation.
*/
class SmoothMeshContactInfo {
struct SmoothMeshContactInfo {
public:
ContactPointInfo contactInfo;
ContactManifoldInfo* contactManifoldInfo;
ContactPointInfo* contactInfo;
bool isFirstShapeTriangle;
Vector3 triangleVertices[3];
bool isUVWZero[3];
/// Constructor
SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
const Vector3& trianglePoint2, const Vector3& trianglePoint3)
: contactInfo(contact) {
SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo,
bool firstShapeTriangle,
const Vector3& trianglePoint1, const Vector3& trianglePoint2,
const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero)
: contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) {
isFirstShapeTriangle = firstShapeTriangle;
triangleVertices[0] = trianglePoint1;
triangleVertices[1] = trianglePoint2;
triangleVertices[2] = trianglePoint3;
isUVWZero[0] = isUZero;
isUVWZero[1] = isVZero;
isUVWZero[2] = isWZero;
}
};
@ -109,7 +119,7 @@ class SmoothMeshContactInfo {
struct ContactsDepthCompare {
bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
{
return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth;
}
};

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

@ -34,12 +34,11 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;

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

@ -61,8 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}

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

@ -46,8 +46,7 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
PROFILE("GJKAlgorithm::testCollision()");
@ -209,7 +208,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
}
// Add a new contact point
contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB);
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
return GJKResult::COLLIDE_IN_MARGIN;
}

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

@ -87,16 +87,13 @@ class GJKAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide.
GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo);
GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
};
}

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

@ -83,8 +83,10 @@ class NarrowPhaseAlgorithm {
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
// TODO : Use the following reportContacts variable in all narrow-phase algorithms
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
};
}

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

@ -44,7 +44,7 @@ using namespace reactphysics3d;
const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()");
@ -145,7 +145,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
// Create the contact info object
contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth,
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
@ -171,7 +171,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
}
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()");
@ -387,7 +387,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
contactManifoldInfo, isCapsuleShape1);
narrowPhaseInfo, isCapsuleShape1);
lastFrameInfo.satIsAxisFacePolyhedron1 = true;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
@ -406,7 +406,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
// Create the contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
@ -477,7 +477,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const {
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
uint firstEdgeIndex = face.edgeIndex;
@ -523,7 +523,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
// Create the contact point
contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
}
@ -543,8 +543,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// Test collision between two convex polyhedrons
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) const {
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo,
bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
@ -869,7 +869,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
// Create a new contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
@ -893,7 +893,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Create the contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
lastFrameInfo.satIsAxisFacePolyhedron1 = false;

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

@ -113,24 +113,24 @@ class SATAlgorithm {
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
/// Test collision between a sphere and a convex mesh
bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, 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
void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const;
NarrowPhaseInfo* narrowPhaseInfo, 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(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
};
}

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

@ -34,7 +34,7 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
@ -86,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
}
// Create the contact info object
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);

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

@ -61,8 +61,7 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}

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

@ -34,12 +34,11 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@ -61,7 +60,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;

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

@ -61,8 +61,7 @@ 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(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}

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

@ -30,8 +30,7 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
@ -62,7 +61,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
return true;

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

@ -61,8 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}

2
src/collision/shapes/BoxShape.h
View File

@ -237,6 +237,8 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
case 4: return Vector3(0, -1, 0);
case 5: return Vector3(0, 1, 0);
}
assert(false);
}
// Return the centroid of the box

17
src/collision/shapes/CollisionShape.h
View File

@ -118,10 +118,6 @@ class CollisionShape {
/// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType);
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2);
// -------------------- Friendship -------------------- //
friend class ProxyShape;
@ -151,19 +147,6 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
// Return the maximum number of contact manifolds allowed in an overlapping
// pair wit the given two collision shape types
inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) {
// If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape
else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
}
}
}
#endif

9
src/constraint/ContactPoint.cpp
View File

@ -37,19 +37,20 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform&
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
mIsRestingContact(false) {
mIsRestingContact(false), mIsObselete(false), mNext(nullptr) {
assert(mPenetrationDepth > decimal(0.0));
// Compute the world position of the contact points
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
mIsObselete = false;
}
// Update the contact point with a new one that is similar (very close)
/// The idea is to keep the cache impulse (for warm starting the contact solver)
void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform) {
void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) {
assert(isSimilarWithContactPoint(contactInfo));
assert(contactInfo->penetrationDepth > decimal(0.0));
@ -62,4 +63,6 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
// Compute the world position of the contact points
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
mIsObselete = false;
}

70
src/constraint/ContactPoint.h
View File

@ -71,6 +71,37 @@ class ContactPoint {
/// Cached penetration impulse
decimal mPenetrationImpulse;
/// True if the contact point is obselete
bool mIsObselete;
/// Pointer to the next contact point in the linked-list
ContactPoint* mNext;
// -------------------- Methods -------------------- //
/// Update the contact point with a new one that is similar (very close)
void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform);
/// Return true if the contact point is similar (close enougth) to another given contact point
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
/// Set to true to make the contact point obselete
void setIsObselete(bool isObselete);
/// Set the next contact point in the linked list
void setNext(ContactPoint* next);
/// Return true if the contact point is obselete
bool getIsObselete() const;
public :
// -------------------- Methods -------------------- //
@ -88,10 +119,6 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
/// Update the contact point with a new one that is similar (very close)
void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform);
/// Return the normal vector of the contact
Vector3 getNormal() const;
@ -110,23 +137,22 @@ class ContactPoint {
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
/// Return true if the contact point is similar (close enougth) to another given contact point
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
/// Return true if the contact is a resting contact
bool getIsRestingContact() const;
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
/// Return the next contact point in the linked list
ContactPoint* getNext() const;
/// Return the penetration depth
decimal getPenetrationDepth() const;
/// Return the number of bytes used by the contact point
size_t getSizeInBytes() const;
// Friendship
friend class ContactManifold;
friend class ContactManifoldSet;
friend class ContactSolver;
};
// Return the normal vector of the contact
@ -180,6 +206,26 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
// Return true if the contact point is obselete
inline bool ContactPoint::getIsObselete() const {
return mIsObselete;
}
// Set to true to make the contact point obselete
inline void ContactPoint::setIsObselete(bool isObselete) {
mIsObselete = isObselete;
}
// Return the next contact point in the linked list
inline ContactPoint* ContactPoint::getNext() const {
return mNext;
}
// Set the next contact point in the linked list
inline void ContactPoint::setNext(ContactPoint* next) {
mNext = next;
}
// Return the penetration depth of the contact
inline decimal ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth;

58
src/engine/CollisionWorld.h
View File

@ -233,64 +233,6 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov
mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
}
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
* the notifyContact() method. This method will called each time a contact
* point is reported.
*/
class CollisionCallback {
public:
struct CollisionCallbackInfo {
public:
const ContactManifoldInfo& contactManifold;
CollisionBody* body1;
CollisionBody* body2;
const ProxyShape* proxyShape1;
const ProxyShape* proxyShape2;
// Constructor
CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2,
const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
contactManifold(manifold), body1(b1), body2(b2),
proxyShape1(proxShape1), proxyShape2(proxShape2) {
}
};
/// Destructor
virtual ~CollisionCallback() {
}
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
};
// Class OverlapCallback
/**
* This class can be used to register a callback for collision overlap queries.
* You should implement your own class inherited from this one and implement
* the notifyOverlap() method. This method will called each time a contact
* point is reported.
*/
class OverlapCallback {
public:
/// Destructor
virtual ~OverlapCallback() {
}
/// This method will be called for each reported overlapping bodies
virtual void notifyOverlap(CollisionBody* collisionBody)=0;
};
}
#endif

8
src/engine/ContactSolver.cpp
View File

@ -148,10 +148,8 @@ void ContactSolver::initializeForIsland(Island* island) {
const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
ContactPoint* externalContact = externalManifold->getContactPoints();
while (externalContact != nullptr) {
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
@ -200,6 +198,8 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
mNbContactPoints++;
externalContact = externalContact->getNext();
}
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);

11
src/engine/DynamicsWorld.cpp
View File

@ -683,9 +683,9 @@ void DynamicsWorld::computeIslands() {
// For each contact manifold in which the current body is involded
ContactManifoldListElement* contactElement;
for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
contactElement = contactElement->next) {
contactElement = contactElement->getNext()) {
ContactManifold* contactManifold = contactElement->contactManifold;
ContactManifold* contactManifold = contactElement->getContactManifold();
assert(contactManifold->getNbContactPoints() > 0);
@ -842,12 +842,13 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// For each contact manifold of the pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* manifold = manifoldSet.getContactManifold(i);
ContactManifold* manifold = manifoldSet.getContactManifolds();
while (manifold != nullptr) {
// Get the contact manifold
contactManifolds.push_back(manifold);
manifold = manifold->getNext();
}
}

10
src/engine/EventListener.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_EVENT_LISTENER_H
// Libraries
#include "collision/ContactManifoldInfo.h"
#include "collision/CollisionCallback.h"
namespace reactphysics3d {
@ -49,17 +49,11 @@ class EventListener {
/// Destructor
virtual ~EventListener() = default;
/// Called when a new contact point is found between two bodies that were separated before
/**
* @param contact Information about the contact
*/
virtual void beginContact(const ContactManifoldInfo& contactManifold) {}
/// Called when a new contact point is found between two bodies
/**
* @param contact Information about the contact
*/
virtual void newContact(const ContactManifoldInfo& contactManifold) {}
virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {}
/// Called at the beginning of an internal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics

100
src/engine/OverlappingPair.cpp
View File

@ -26,12 +26,108 @@
// Libraries
#include <cassert>
#include "OverlappingPair.h"
#include "collision/ContactManifoldInfo.h"
using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) {
Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator)
: mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr),
mTempMemoryAllocator(temporaryMemoryAllocator) {
}
// Create a new potential contact manifold using contact-points from narrow-phase
void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) {
assert(narrowPhaseInfo->contactPoints != nullptr);
// For each potential contact point to add
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
while (contactPoint != nullptr) {
ContactPointInfo* nextContactPoint = contactPoint->next;
// Compute the contact normal id
short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal);
// 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)
ContactManifoldInfo* manifold = mPotentialContactManifolds;
bool similarManifoldFound = false;
while(manifold != nullptr) {
// If we have found a corresponding manifold for the new contact point
// (a manifold with a similar contact normal direction)
if (manifold->getContactNormalId() == contactNormalId) {
// Add the contact point to the manifold
manifold->addContactPoint(contactPoint, contactNormalId);
similarManifoldFound = true;
break;
}
manifold = manifold->getNext();
}
// If we have not found an existing manifold with a similar contact normal
if (!similarManifoldFound) {
// Create a new potential contact manifold
ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo)))
ContactManifoldInfo(mTempMemoryAllocator);
// Add the manifold into the linked-list of potential contact manifolds
manifoldInfo->mNext = mPotentialContactManifolds;
mPotentialContactManifolds = manifoldInfo;
// Add the contact point to the manifold
manifoldInfo->addContactPoint(contactPoint, contactNormalId);
}
contactPoint = nextContactPoint;
}
// All the contact point info of the narrow-phase info have been moved
// into the potential contacts of the overlapping pair
narrowPhaseInfo->contactPoints = nullptr;
}
// Clear all the potential contact manifolds
void OverlappingPair::clearPotentialContactManifolds() {
// Do we need to release memory
if (mTempMemoryAllocator.isReleaseNeeded()) {
ContactManifoldInfo* element = mPotentialContactManifolds;
while(element != nullptr) {
// Remove the proxy collision shape
ContactManifoldInfo* elementToRemove = element;
element = element->getNext();
// Delete the element
elementToRemove->~ContactManifoldInfo();
mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
}
}
mPotentialContactManifolds = nullptr;
}
// Reduce the number of contact points of all the potential contact manifolds
void OverlappingPair::reducePotentialContactManifolds() {
// For each potential contact manifold
ContactManifoldInfo* manifold = mPotentialContactManifolds;
while (manifold != nullptr) {
// Reduce the number of contact points of the manifold
manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform());
manifold = manifold->getNext();
}
}

69
src/engine/OverlappingPair.h
View File

@ -102,13 +102,19 @@ class OverlappingPair {
/// Collision information about the last frame (for temporal coherence)
LastFrameCollisionInfo mLastFrameCollisionInfo;
/// Linked-list of potential contact manifold
ContactManifoldInfo* mPotentialContactManifolds;
/// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
Allocator& mTempMemoryAllocator;
public:
// -------------------- Methods -------------------- //
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, PoolAllocator& memoryAllocator);
Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator);
/// Destructor
~OverlappingPair() = default;
@ -125,9 +131,6 @@ class OverlappingPair {
/// Return the pointer to second body
ProxyShape* getShape2() const;
/// Add a contact manifold
void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
/// Return the last frame collision info
LastFrameCollisionInfo& getLastFrameCollisionInfo();
@ -137,8 +140,32 @@ class OverlappingPair {
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
/// Clear the contact points of the contact manifold
void clearContactPoints();
/// Clear all the potential contact manifolds
void clearPotentialContactManifolds();
/// Add potential contact-points from narrow-phase into potential contact manifolds
void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo);
/// Add a contact to the contact manifold
void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
/// Return a reference to the temporary memory allocator
Allocator& getTemporaryAllocator();
/// Return true if one of the shapes of the pair is a concave shape
bool hasConcaveShape() const;
/// Return a pointer to the first potential contact manifold in the linked-list
ContactManifoldInfo* getPotentialContactManifolds();
/// Reduce the number of contact points of all the potential contact manifolds
void reducePotentialContactManifolds();
/// Make the contact manifolds and contact points obselete
void makeContactsObselete();
/// Clear the obselete contact manifold and contact points
void clearObseleteManifoldsAndContactPoints();
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@ -162,7 +189,7 @@ inline ProxyShape* OverlappingPair::getShape2() const {
}
// Add a contact to the contact manifold
inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
@ -181,6 +208,12 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
return mContactManifoldSet;
}
// Make the contact manifolds and contact points obselete
inline void OverlappingPair::makeContactsObselete() {
mContactManifoldSet.makeContactsObselete();
}
// Return the pair of bodies index
inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0);
@ -205,9 +238,25 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
return indexPair;
}
// Clear the contact points of the contact manifold
inline void OverlappingPair::clearContactPoints() {
mContactManifoldSet.clear();
// Return a reference to the temporary memory allocator
inline Allocator& OverlappingPair::getTemporaryAllocator() {
return mTempMemoryAllocator;
}
// Return true if one of the shapes of the pair is a concave shape
inline bool OverlappingPair::hasConcaveShape() const {
return !getShape1()->getCollisionShape()->isConvex() ||
!getShape2()->getCollisionShape()->isConvex();
}
// Return a pointer to the first potential contact manifold in the linked-list
inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
return mPotentialContactManifolds;
}
// Clear the obselete contact manifold and contact points
inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() {
mContactManifoldSet.clearObseleteManifoldsAndContactPoints();
}
}

3
src/memory/Allocator.h
View File

@ -49,6 +49,9 @@ class Allocator {
/// Release previously allocated memory.
virtual void release(void* pointer, size_t size)=0;
/// Return true if memory needs to be release with this allocator
virtual bool isReleaseNeeded() const=0;
};
}

8
src/memory/PoolAllocator.h
View File

@ -138,8 +138,16 @@ class PoolAllocator : public Allocator {
/// Release previously allocated memory.
virtual void release(void* pointer, size_t size) override;
/// Return true if memory needs to be release with this allocator
virtual bool isReleaseNeeded() const override;
};
// Return true if memory needs to be release with this allocator
inline bool PoolAllocator::isReleaseNeeded() const {
return true;
}
}
#endif

9
src/memory/SingleFrameAllocator.h
View File

@ -72,10 +72,15 @@ class SingleFrameAllocator : public Allocator {
/// Reset the marker of the current allocated memory
virtual void reset();
/// Return true if memory needs to be release with this allocator
virtual bool isReleaseNeeded() const override;
};
// Return true if memory needs to be release with this allocator
inline bool SingleFrameAllocator::isReleaseNeeded() const {
return false;
}
}
#endif

2
src/reactphysics3d.h
View File

@ -58,6 +58,8 @@
#include "collision/PolyhedronMesh.h"
#include "collision/TriangleVertexArray.h"
#include "collision/PolygonVertexArray.h"
#include "collision/CollisionCallback.h"
#include "collision/OverlapCallback.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"

40
testbed/scenes/collisiondetection/CollisionDetectionScene.h
View File

@ -79,26 +79,34 @@ class ContactManager : public rp3d::CollisionCallback {
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
// For each contact point
rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo();
while (contactPointInfo != nullptr) {
// For each contact manifold
rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements;
while (manifoldElement != nullptr) {
// Contact normal
rp3d::Vector3 normal = contactPointInfo->normal;
openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
// Get the contact manifold
rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold();
rp3d::Vector3 point1 = contactPointInfo->localPoint1;
point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
openglframework::Vector3 position1(point1.x, point1.y, point1.z);
mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
// For each contact point
rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints();
while (contactPoint != nullptr) {
rp3d::Vector3 point2 = contactPointInfo->localPoint2;
point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
openglframework::Vector3 position2(point2.x, point2.y, point2.z);
mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
// Contact normal
rp3d::Vector3 normal = contactPoint->getNormal();
openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
contactPointInfo = contactPointInfo->next;
rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1();
point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
openglframework::Vector3 position1(point1.x, point1.y, point1.z);
mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2();
point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
openglframework::Vector3 position2(point2.x, point2.y, point2.z);
mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
contactPoint = contactPoint->getNext();
}
}
}

6
testbed/src/SceneDemo.cpp
View File

@ -342,14 +342,16 @@ std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn
const rp3d::ContactManifold* manifold = *it;
// For each contact point of the manifold
for (uint i=0; i<manifold->getNbContactPoints(); i++) {
rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
while (contactPoint != nullptr) {
rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i);
rp3d::Vector3 point = contactPoint->getWorldPointOnBody1();
rp3d::Vector3 normalWorld = contactPoint->getNormal();
openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
contactPoints.push_back(contact);
contactPoint = contactPoint->getNext();
}
}