Add ContactManifoldSet to allow several ContactManifolds per overlapping pair

This commit is contained in:
Daniel Chappuis 2015-10-08 21:28:37 +02:00
parent 535bc0bcc1
commit ec5e41c19e
11 changed files with 284 additions and 77 deletions

View File

@ -119,26 +119,32 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
continue;
}
// For each contact manifold of the overlapping pair
ContactManifold* manifold = pair->getContactManifold();
for (uint i=0; i<manifold->getNbContactPoints(); i++) {
// For each contact manifold set of the overlapping pair
ContactManifoldSet* manifoldSet = pair->getContactManifoldSet();
for (uint j=0; j<manifoldSet->getNbContactManifolds(); j++) {
ContactPoint* contactPoint = manifold->getContactPoint(i);
ContactManifold* manifold = manifoldSet->getContactManifold(j);
// Create the contact info object for the contact
ContactPointInfo* contactInfo = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(manifold->getShape1(), manifold->getShape2(),
contactPoint->getNormal(),
contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2());
// For each contact manifold of the manifold set
for (uint i=0; i<manifold->getNbContactPoints(); i++) {
// Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(*contactInfo);
ContactPoint* contactPoint = manifold->getContactPoint(i);
// Delete and remove the contact info from the memory allocator
contactInfo->~ContactPointInfo();
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
// Create the contact info object for the contact
ContactPointInfo* contactInfo = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(manifold->getShape1(), manifold->getShape2(),
contactPoint->getNormal(),
contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2());
// Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(*contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->~ContactPointInfo();
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
}
}
}
@ -162,6 +168,9 @@ void CollisionDetection::computeBroadPhase() {
void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()");
// Clear the set of overlapping pairs in narrow-phase contact
mContactOverlappingPairs.clear();
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
@ -252,6 +261,9 @@ void CollisionDetection::computeNarrowPhase() {
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
}
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
// Compute the narrow-phase collision detection
@ -259,6 +271,8 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
mContactOverlappingPairs.clear();
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@ -361,6 +375,9 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
}
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
// Allow the broadphase to notify the collision detection about an overlapping pair.
@ -382,9 +399,13 @@ 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->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, mWorld->mMemoryAllocator);
OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
assert(newPair != NULL);
std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
mOverlappingPairs.insert(make_pair(pairID, newPair));
@ -434,39 +455,55 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
// Add the contact to the contact manifold set of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add the contact manifold into the list of contact manifolds
// of the two bodies involved in the contact
// Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = overlappingPair;
}
// TODO : It seems that we add the same manifold multiple times to the same body
// in case we call createContact() multiple time for different contact points from the
// same manifold. Check that it is not the case.
addContactManifoldToBody(overlappingPair->getContactManifold(),
overlappingPair->getShape1()->getBody(),
overlappingPair->getShape2()->getBody());
void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// Add all the contact manifolds of the pair into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(it->second);
}
}
// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
// in the corresponding contact
void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody* body1, CollisionBody* body2) {
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
assert(contactManifold != NULL);
assert(pair != NULL);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
CollisionBody* body1 = pair->getShape1()->getBody();
CollisionBody* body2 = pair->getShape2()->getBody();
ContactManifoldSet* manifoldSet = pair->getContactManifoldSet();
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
// 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);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
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->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}
}
// Delete all the contact points in the currently overlapping pairs

View File

@ -78,6 +78,9 @@ class CollisionDetection {
/// Broad-phase overlapping pairs
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
/// Overlapping pairs in contact (during the current Narrow-phase collision detection)
std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
/// Broad-phase algorithm
BroadPhaseAlgorithm mBroadPhaseAlgorithm;
@ -111,14 +114,16 @@ class CollisionDetection {
/// Add a contact manifold to the linked list of contact manifolds of the two bodies
/// involed in the corresponding contact.
void addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody *body1, CollisionBody *body2);
void addContactManifoldToBody(OverlappingPair* pair);
/// Delete all the contact points in the currently overlapping pairs
void clearContactPoints();
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
/// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies();
public :

View File

@ -31,9 +31,10 @@ using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2), mNbContactPoints(0), mFrictionImpulse1(0.0),
mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
MemoryAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
}

View File

@ -98,6 +98,9 @@ class ContactManifold {
/// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId;
/// Number of contacts in the cache
uint mNbContactPoints;
@ -147,9 +150,6 @@ class ContactManifold {
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
/// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const;
public:
@ -157,7 +157,7 @@ class ContactManifold {
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator);
MemoryAllocator& memoryAllocator, short int normalDirectionId);
/// Destructor
~ContactManifold();
@ -174,6 +174,9 @@ 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;
/// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact);
@ -219,6 +222,12 @@ class ContactManifold {
/// 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;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@ -246,6 +255,11 @@ 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 uint ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
@ -323,6 +337,20 @@ inline Vector3 ContactManifold::getAverageContactNormal() const {
return averageNormal.getUnit();
}
// Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
for (int i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
}
return largestDepth;
}
}
#endif

View File

@ -39,38 +39,127 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
// Destructor
ContactManifoldSet::~ContactManifoldSet() {
// Clear all the contact manifolds
clear();
}
// Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
// If there is no contact manifold yet
if (mNbManifolds == 0) {
createManifold();
createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact);
return;
}
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
// Add the contact point to that similar manifold
mManifolds[similarManifoldIndex]->addContactPoint(contact);
return;
}
// If the maximum number of manifold has not been reached yet
if (mNbManifolds < mNbMaxManifolds) {
// Create a new manifold for the contact point
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
return;
}
// 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 keep the manifolds
// with the largest contact depth among their points
int smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth();
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
smallestDepthIndex = i;
}
}
// If we do not want to keep to new manifold (not created yet) with the
// new contact point
if (smallestDepthIndex == -1) {
return;
}
assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
// Here we need to replace an existing manifold with a new one (that contains
// the new contact point)
removeManifold(smallestDepthIndex);
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
return;
}
// 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(const Vector3& normal) {
int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
decimal maxDotProduct;
int indexManifold = -1;
// Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<mNbManifolds; i++) {
decimal dotProduct = normal.dot(mManifolds[i]->getAverageContactNormal());
if (dotProduct > maxDotProduct) {
maxDotProduct = dotProduct;
indexManifold = i;
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i;
}
}
return indexManifold;
return -1;
}
// 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 {
assert(normal.lengthSquare() > MACHINE_EPSILON);
int faceNo;
decimal u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
faceNo = normalScaled.x > 0 ? 0 : 1;
u = normalScaled.y;
v = normalScaled.z;
}
else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
faceNo = normalScaled.y > 0 ? 2 : 3;
u = normalScaled.x;
v = normalScaled.z;
}
else {
faceNo = normalScaled.z > 0 ? 4 : 5;
u = normalScaled.x;
v = normalScaled.y;
}
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
return faceNo * 200 + indexU * nbSubDivInFace + indexV;
}
// Update the contact manifolds
@ -84,14 +173,20 @@ void ContactManifoldSet::update(const Transform& transform1, const Transform& tr
// Clear the contact manifold set
void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) {
removeManifold(i);
}
assert(mNbManifolds == 0);
}
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold() {
assert(mNbManifolds < MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET);
void ContactManifoldSet::createManifold(short int normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(mShape1, mShape2, mMemoryAllocator);
ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
mNbManifolds++;
}
@ -104,5 +199,9 @@ void ContactManifoldSet::removeManifold(int index) {
mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
for (int i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1];
}
mNbManifolds--;
}

View File

@ -32,7 +32,8 @@
namespace reactphysics3d {
// Constants
const uint MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 4; // N Number for the N x N subdivisions of the cubemap
// Class ContactManifoldSet
/**
@ -68,11 +69,19 @@ class ContactManifoldSet {
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
void createManifold();
void createManifold(short normalDirectionId);
/// 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;
public:
// -------------------- Methods -------------------- //

View File

@ -106,6 +106,10 @@ 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;
@ -125,6 +129,19 @@ inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
return shapeType != CONCAVE_MESH;
}
// 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

View File

@ -128,10 +128,13 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
/// followin constant with the linear velocity and the elapsed time between two frames.
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
/// This value is used to compare the normal of a contact point and the average
/// normal of a whole contact manifold to decide if the point should be part
/// of the manifold or not
const decimal MANIFOLD_SIMILAR_NORMAL_DOTPRODUCT_THRESHOLD = 0.99f;
/// Maximum number of contact manifolds in an overlapping pair that involves two
/// convex collision shapes.
const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// Maximum number of contact manifolds in an overlapping pair that involves at
/// least one concave collision shape.
const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
}

View File

@ -984,8 +984,15 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
OverlappingPair* pair = it->second;
// Get the contact manifold
contactManifolds.push_back(pair->getContactManifold());
// For each contact manifold of the pair
ContactManifoldSet* manifoldSet = pair->getContactManifoldSet();
for (int i=0; i<manifoldSet->getNbContactManifolds(); i++) {
ContactManifold* manifold = manifoldSet->getContactManifold(i);
// Get the contact manifold
contactManifolds.push_back(manifold);
}
}
// Return all the contact manifold

View File

@ -31,9 +31,9 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator)
int nbMaxContactManifolds, MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2),
mContactManifoldSet(shape1, shape2, memoryAllocator),
mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
}

View File

@ -76,7 +76,8 @@ class OverlappingPair {
// -------------------- Methods -------------------- //
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator);
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, MemoryAllocator& memoryAllocator);
/// Destructor
~OverlappingPair();
@ -102,8 +103,8 @@ class OverlappingPair {
/// Return the number of contacts in the cache
uint getNbContactPoints() const;
/// Return the contact manifold
ContactManifold* getContactManifold();
/// Return the contact manifold set
ContactManifoldSet* getContactManifoldSet();
/// Clear the contact points of the contact manifold
void clearContactPoints();
@ -157,7 +158,7 @@ inline uint OverlappingPair::getNbContactPoints() const {
}
// Return the contact manifold
inline ContactManifold* OverlappingPair::getContactManifoldSet() {
inline ContactManifoldSet* OverlappingPair::getContactManifoldSet() {
return &mContactManifoldSet;
}