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

Add the BroadPhasePair class

Browse Source
This commit is contained in:
Daniel Chappuis 2012-09-26 23:07:40 +02:00
parent 2ed2b2a54f
commit 4c0c285174
17 changed files with 316 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/body/Body.h
View File

@ -56,7 +56,7 @@ class Body {
bool operator<(const Body& body2) const; // Smaller than operator
bool operator>(const Body& body2) const; // Larger than operator
bool operator==(const Body& body2) const; // Equal operator
bool operator!=(const Body& body2) const; // Equal operator
bool operator!=(const Body& body2) const; // Not equal operator
};
// Return the id of the body
@ -79,7 +79,7 @@ inline bool Body::operator==(const Body& body2) const {
return (id == body2.id);
}
// Equal operator
// Not equal operator
inline bool Body::operator!=(const Body& body2) const {
return (id != body2.id);
}

40
src/collision/BroadPhasePair.cpp Normal file
View File

@ -0,0 +1,40 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2012 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 "BroadPhasePair.h"
using namespace reactphysics3d;
// Constructor
BroadPhasePair::BroadPhasePair(CollisionBody* body1, CollisionBody* body2)
: body1(body1), body2(body2), previousSeparatingAxis(Vector3(1,1,1)) {
}
// Destructor
BroadPhasePair::~BroadPhasePair() {
}

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

@ -0,0 +1,89 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2012 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 BROAD_PHASE_PAIR_H
#define BROAD_PHASE_PAIR_H
// Libraries
#include "../body/CollisionBody.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// struct BroadPhasePair
// This structure represents a pair of bodies
// during the broad-phase collision detection
struct BroadPhasePair {
public:
CollisionBody* body1; // Pointer to the first body
CollisionBody* body2; // Pointer to the second body
Vector3 previousSeparatingAxis; // Previous cached separating axis
BroadPhasePair(CollisionBody* body1, CollisionBody* body2); // Constructor
~BroadPhasePair(); // Destructor
bodyindexpair getBodiesIndexPair() const; // Return the pair of bodies index
// Operators
bool operator<(const BroadPhasePair& broadPhasePair2) const; // Smaller than operator
bool operator>(const BroadPhasePair& broadPhasePair2) const; // Larger than operator
bool operator==(const BroadPhasePair& broadPhasePair2) const; // Equal operator
bool operator!=(const BroadPhasePair& broadPhasePair2) const; // Not equal operator
};
// Return the pair of bodies index
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
// Construct the pair of body index
bodyindexpair indexPair = body1->getID() < body2->getID() ? std::make_pair(body1->getID(), body2->getID()) :
std::make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);
return indexPair;
}
// Smaller than operator
inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const {
return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2));
}
// Larger than operator
inline bool BroadPhasePair::operator>(const BroadPhasePair& broadPhasePair2) const {
return (body1 > broadPhasePair2.body1 ? true : (body2 > broadPhasePair2.body2));
}
// Equal operator
inline bool BroadPhasePair::operator==(const BroadPhasePair& broadPhasePair2) const {
return (body1 == broadPhasePair2.body1 && body2 == broadPhasePair2.body2);
}
// Not equal operator
inline bool BroadPhasePair::operator!=(const BroadPhasePair& broadPhasePair2) const {
return (body1 != broadPhasePair2.body1 || body2 != broadPhasePair2.body2);
}
}
#endif

84
src/collision/CollisionDetection.cpp
View File

@ -56,12 +56,6 @@ CollisionDetection::CollisionDetection(PhysicsWorld* world)
// Destructor
CollisionDetection::~CollisionDetection() {
// Delete the remaining overlapping pairs
for (map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator it=overlappingPairs.begin(); it != overlappingPairs.end(); it++) {
// Delete the overlapping pair
(*it).second->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject((*it).second);
}
// Delete the broad-phase algorithm
delete broadPhaseAlgorithm;
@ -107,32 +101,31 @@ void CollisionDetection::computeBroadPhase() {
broadPhaseAlgorithm->updateObject(*it, *((*it)->getAABB()));
}
}
// TODO : DELETE THIS
//std::cout << "Nb overlapping pairs : " << overlappingPairs.size() << std::endl;
//std::cout << "Nb active pairs in pair manager : " << broadPhaseAlgorithm->getNbOverlappingPairs() << std::endl;
}
// Compute the narrow-phase collision detection
bool CollisionDetection::computeNarrowPhase() {
bool collisionExists = false;
map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator it;
map<bodyindexpair, BroadPhasePair*>::iterator it;
// For each possible collision pair of bodies
for (it = overlappingPairs.begin(); it != overlappingPairs.end(); it++) {
ContactInfo* contactInfo = NULL;
ContactInfo* contactInfo = 0;
CollisionBody* const body1 = (*it).second->getBody1();
CollisionBody* const body2 = (*it).second->getBody2();
BroadPhasePair* pair = (*it).second;
assert(pair);
CollisionBody* const body1 = pair->body1;
CollisionBody* const body2 = pair->body2;
// Update the contact cache of the overlapping pair
(*it).second->update();
world->updateOverlappingPair(pair);
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(body1->getCollisionShape(), body2->getCollisionShape());
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm.setCurrentOverlappingPair((*it).second);
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
// Use the narrow-phase collision detection algorithm to check if there really is a collision
if (narrowPhaseAlgorithm.testCollision(body1->getCollisionShape(), body1->getTransform(),
@ -140,24 +133,12 @@ bool CollisionDetection::computeNarrowPhase() {
assert(contactInfo);
collisionExists = true;
RigidBody* const rigidBody1 = dynamic_cast<RigidBody* const>(body1);
RigidBody* const rigidBody2 = dynamic_cast<RigidBody* const>(body2);
// Notify the world about the new narrow-phase contact
world->notifyNewContact(pair, contactInfo);
// Create a new contact
Contact* contact = new (memoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo);
// Delete and remove the contact info from the memory pool
contactInfo->ContactInfo::~ContactInfo();
memoryPoolContactInfos.freeObject(contactInfo);
// Add the contact to the contact cache of the corresponding overlapping pair
(*it).second->addContact(contact);
// Add all the contacts in the contact cache of the two bodies
// to the set of constraints in the physics world
for (uint i=0; i<(*it).second->getNbContacts(); i++) {
world->addConstraint((*it).second->getContact(i));
}
}
}
@ -166,34 +147,37 @@ bool CollisionDetection::computeNarrowPhase() {
// Allow the broadphase to notify the collision detection about an overlapping pair
// This method is called by a broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPair) {
// Construct the pair of body index
pair<bodyindex, bodyindex> indexPair = addedPair->body1->getID() < addedPair->body2->getID() ? make_pair(addedPair->body1->getID(), addedPair->body2->getID()) :
make_pair(addedPair->body2->getID(), addedPair->body1->getID());
assert(indexPair.first != indexPair.second);
// Get the pair of body index
bodyindexpair indexPair = addedPair->getBodiesIndexPair();
// Create the corresponding broad-phase pair object
BroadPhasePair* broadPhasePair = new (memoryPoolOverlappingPairs.allocateObject()) BroadPhasePair(addedPair->body1, addedPair->body2);
// Add the pair into the set of overlapping pairs (if not there yet)
OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(addedPair->body1, addedPair->body2, memoryPoolContacts);
pair<map<pair<bodyindex, bodyindex>, OverlappingPair*>::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, newPair));
// If the overlapping pair was already in the set of overlapping pair
if (!check.second) {
// Delete the new pair
newPair->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(newPair);
}
pair<map<bodyindexpair, BroadPhasePair*>::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, broadPhasePair));
assert(check.second);
// Notify the world about the new broad-phase overlapping pair
world->notifyAddedOverlappingPair(broadPhasePair);
}
// Allow the broadphase to notify the collision detection about a removed overlapping pair
void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(BodyPair* removedPair) {
// Construct the pair of body index
pair<bodyindex, bodyindex> indexPair = removedPair->body1->getID() < removedPair->body2->getID() ? make_pair(removedPair->body1->getID(), removedPair->body2->getID()) :
make_pair(removedPair->body2->getID(), removedPair->body1->getID());
// Get the pair of body index
bodyindexpair indexPair = removedPair->getBodiesIndexPair();
// Get the broad-phase pair
BroadPhasePair* broadPhasePair = overlappingPairs[indexPair];
assert(broadPhasePair);
// Notify the world about the removed broad-phase pair
world->notifyRemovedOverlappingPair(broadPhasePair);
// Remove the overlapping pair from the memory pool
overlappingPairs[indexPair]->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(overlappingPairs[indexPair]);
broadPhasePair->BroadPhasePair::~BroadPhasePair();
memoryPoolOverlappingPairs.freeObject(broadPhasePair);
overlappingPairs.erase(indexPair);
}

26
src/collision/CollisionDetection.h
View File

@ -28,8 +28,8 @@
// Libraries
#include "../body/CollisionBody.h"
#include "OverlappingPair.h"
#include "broadphase/BroadPhaseAlgorithm.h"
#include "BroadPhasePair.h"
#include "../memory/MemoryPool.h"
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h"
@ -60,16 +60,13 @@ class CollisionDetection {
private :
PhysicsWorld* world; // Pointer to the physics world
std::map<std::pair<bodyindex, bodyindex>, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
std::set<std::pair<bodyindex, bodyindex> > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step
std::set<std::pair<bodyindex, bodyindex> > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step
std::map<bodyindexpair, BroadPhasePair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
GJKAlgorithm narrowPhaseGJKAlgorithm; // Narrow-phase GJK algorithm
SphereVsSphereAlgorithm narrowPhaseSphereVsSphereAlgorithm; // Narrow-phase Sphere vs Sphere algorithm
MemoryPool<Contact> memoryPoolContacts; // Memory pool for the contacts
MemoryPool<OverlappingPair> memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs
MemoryPool<ContactInfo> memoryPoolContactInfos; // Memory pool for the contact info
MemoryPool<BroadPhasePair> memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs
void computeBroadPhase(); // Compute the broad-phase collision detection
bool computeNarrowPhase(); // Compute the narrow-phase collision detection
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1,
@ -81,22 +78,11 @@ class CollisionDetection {
void addBody(CollisionBody* body); // Add a body to the collision detection
void removeBody(CollisionBody* body); // Remove a body from the collision detection
OverlappingPair* getOverlappingPair(bodyindex body1ID, bodyindex body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection
void broadPhaseNotifyAddedOverlappingPair(const BroadPhasePair* pair); // Allow the broadphase to notify the collision detection about a new overlapping pair
void broadPhaseNotifyRemovedOverlappingPair(const BroadPhasePair* pair); // Allow the broadphase to notify the collision detection about a removed overlapping pair
void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair); // Allow the broadphase to notify the collision detection about a new overlapping pair
void broadPhaseNotifyRemovedOverlappingPair(BodyPair* pair); // Allow the broadphase to notify the collision detection about a removed overlapping pair
};
// Return an overlapping pair of bodies according to the given bodies ID
// The method returns null if the pair of bodies is not overlapping
inline OverlappingPair* CollisionDetection::getOverlappingPair(bodyindex body1ID, bodyindex body2ID) {
std::pair<bodyindex, bodyindex> pair = (body1ID < body2ID) ? std::make_pair(body1ID, body2ID) : std::make_pair(body2ID, body1ID);
if (overlappingPairs.count(pair) == 1) {
return overlappingPairs[pair];
}
return 0;
}
// Select the narrow-phase collision algorithm to use given two collision shapes
inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1, CollisionShape* collisionShape2) {

8
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -66,8 +66,8 @@ class BroadPhaseAlgorithm {
virtual void removeObject(CollisionBody* body)=0; // Notify the broad-phase about an object that has been removed from the world
virtual void updateObject(CollisionBody* body, const AABB& aabb)=0; // Notify the broad-phase that the AABB of an object has changed
BroadPhasePair* beginOverlappingPairsPointer() const; // Return a pointer to the first active pair (used to iterate over the active pairs)
BroadPhasePair* endOverlappingPairsPointer() const; // Return a pointer to the last active pair (used to iterate over the active pairs)
BodyPair* beginOverlappingPairsPointer() const; // Return a pointer to the first active pair (used to iterate over the active pairs)
BodyPair* endOverlappingPairsPointer() const; // Return a pointer to the last active pair (used to iterate over the active pairs)
};
// TODO : DELETE THIS METHOD
@ -77,12 +77,12 @@ inline uint BroadPhaseAlgorithm::getNbOverlappingPairs() const {
// Return a pointer to the first active pair (used to iterate over the overlapping pairs)
inline BroadPhasePair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
inline BodyPair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
return pairManager.beginOverlappingPairsPointer();
}
// Return a pointer to the last active pair (used to iterate over the overlapping pairs)
inline BroadPhasePair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const {
inline BodyPair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const {
return pairManager.endOverlappingPairsPointer();
}

18
src/collision/broadphase/PairManager.cpp
View File

@ -56,7 +56,7 @@ PairManager::~PairManager() {
// that pair. If the pair to add does not already exist in the set of
// overlapping pairs, it will be created and if it already exists, we only
// return a pointer to that pair.
BroadPhasePair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2) {
BodyPair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2) {
// Sort the bodies to have the body with smallest ID first
sortBodiesUsingID(body1, body2);
@ -69,7 +69,7 @@ BroadPhasePair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2)
uint hashValue = computeHashBodies(id1, id2) & hashMask;
// Try to find the pair in the current overlapping pairs.
BroadPhasePair* pair = findPairWithHashValue(id1, id2, hashValue);
BodyPair* pair = findPairWithHashValue(id1, id2, hashValue);
// If the pair is already in the set of overlapping pairs
if (pair) {
@ -93,7 +93,7 @@ BroadPhasePair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2)
}
// Create the new overlapping pair
BroadPhasePair* newPair = &overlappingPairs[nbOverlappingPairs];
BodyPair* newPair = &overlappingPairs[nbOverlappingPairs];
newPair->body1 = body1;
newPair->body2 = body2;
@ -119,7 +119,7 @@ bool PairManager::removePair(bodyindex id1, bodyindex id2) {
const uint hashValue = computeHashBodies(id1, id2) & hashMask;
// Find the pair to remove
const BroadPhasePair* pair = findPairWithHashValue(id1, id2, hashValue);
BodyPair* pair = findPairWithHashValue(id1, id2, hashValue);
// If we have not found the pair
if (!pair) {
@ -184,7 +184,7 @@ void PairManager::removePairWithHashValue(bodyindex id1, bodyindex id2, luint ha
// now free because of the pair we want to remove
// Get the last pair
const BroadPhasePair* lastPair = &overlappingPairs[indexLastPair];
const BodyPair* lastPair = &overlappingPairs[indexLastPair];
const uint lastPairHashValue = computeHashBodies(lastPair->body1->getID(), lastPair->body2->getID()) & hashMask;
// Compute the initial offset of the last pair
@ -223,7 +223,7 @@ void PairManager::removePairWithHashValue(bodyindex id1, bodyindex id2, luint ha
}
// Look for a pair in the set of overlapping pairs
BroadPhasePair* PairManager::lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const {
BodyPair* PairManager::lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const {
// Look for the pair in the set of overlapping pairs
bodyindex offset = hashTable[hashValue];
@ -256,7 +256,7 @@ void PairManager::reallocatePairs() {
}
// Reallocate the overlapping pairs
BroadPhasePair* newOverlappingPairs = (BroadPhasePair*) malloc(nbElementsHashTable * sizeof(BroadPhasePair));
BodyPair* newOverlappingPairs = (BodyPair*) malloc(nbElementsHashTable * sizeof(BodyPair));
bodyindex* newOffsetNextPair = (bodyindex*) malloc(nbElementsHashTable * sizeof(bodyindex));
assert(newOverlappingPairs);
@ -264,8 +264,8 @@ void PairManager::reallocatePairs() {
// If there is already some overlapping pairs
if (nbOverlappingPairs) {
// Copy the pairs to the new place
memcpy(newOverlappingPairs, overlappingPairs, nbOverlappingPairs * sizeof(BroadPhasePair));
// Copy the pairs to the new location
memcpy(newOverlappingPairs, overlappingPairs, nbOverlappingPairs * sizeof(BodyPair));
}
// Recompute the hash table with the new hash values

56
src/collision/broadphase/PairManager.h
View File

@ -28,6 +28,7 @@
// Libraries
#include "../../body/CollisionBody.h"
#include <utility>
// Namespace ReactPhysics3D
@ -35,15 +36,26 @@ namespace reactphysics3d {
// Declaration
class CollisionDetection;
// Structure BroadPhasePair that represents a pair of bodies
// struct BodyPair
// This structure represents a pair of bodies
// during the broad-phase collision detection
struct BroadPhasePair {
struct BodyPair {
public:
CollisionBody* body1; // Pointer to the first body
CollisionBody* body2; // Pointer to the second body
};
// Return the pair of bodies index
bodyindexpair getBodiesIndexPair() const {
// Construct the pair of body index
bodyindexpair indexPair = body1->getID() < body2->getID() ? std::make_pair(body1->getID(), body2->getID()) :
std::make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);
return indexPair;
}
};
/* --------------------------------------------------------------------
Class PairManager :
@ -63,23 +75,23 @@ class PairManager {
bodyindex* offsetNextPair; // Array that contains for each offset, the offset of the next pair with
// the same hash value
// for a given same hash value
BroadPhasePair* overlappingPairs; // Array that contains the currently active pairs
BodyPair* overlappingPairs; // Array that contains the currently active pairs
static bodyindex INVALID_INDEX; // Invalid ID
CollisionDetection& collisionDetection; // Reference to the collision detection
void sortBodiesUsingID(CollisionBody*& body1, CollisionBody*& body2) const; // Sort the bodies according to their IDs (smallest ID first)
void sortIDs(bodyindex& id1, bodyindex& id2) const; // Sort the IDs (smallest ID first)
bool isDifferentPair(const BroadPhasePair& pair1, bodyindex pair2ID1,
bool isDifferentPair(const BodyPair& pair1, bodyindex pair2ID1,
bodyindex pair2ID2) const; // Return true if pair1 and pair2 are the same
uint computeHashBodies(uint id1, uint id2) const; // Compute the hash value of two bodies using their IDs
int computeHash32Bits(int key) const; // This method returns an hash value for a 32 bits key
luint computeNextPowerOfTwo(luint number) const; // Return the next power of two
void reallocatePairs(); // Reallocate memory for more pairs
void shrinkMemory(); // Shrink the allocated memory
bodyindex computePairOffset(const BroadPhasePair* pair) const; // Compute the offset of a given pair
BroadPhasePair* lookForAPair(bodyindex id1, bodyindex id2,
bodyindex computePairOffset(const BodyPair* pair) const; // Compute the offset of a given pair
BodyPair* lookForAPair(bodyindex id1, bodyindex id2,
luint hashValue) const; // Look for a pair in the set of overlapping pairs
BroadPhasePair* findPairWithHashValue(bodyindex id1, bodyindex id2,
BodyPair* findPairWithHashValue(bodyindex id1, bodyindex id2,
luint hashValue) const; // Find a pair given two body IDs and an hash value
void removePairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue,
bodyindex indexPair); // Remove a pair from the set of active pair
@ -88,14 +100,14 @@ class PairManager {
~PairManager(); // Destructor
bodyindex getNbOverlappingPairs() const; // Return the number of active pairs
BroadPhasePair* addPair(CollisionBody* body1, CollisionBody* body2); // Add a pair of bodies in the pair manager
BodyPair* addPair(CollisionBody* body1, CollisionBody* body2); // Add a pair of bodies in the pair manager
bool removePair(bodyindex id1, bodyindex id2); // Remove a pair of bodies from the pair manager
BroadPhasePair* findPair(bodyindex id1, bodyindex id2) const; // Find a pair given two body IDs
BroadPhasePair* beginOverlappingPairsPointer() const; // Return a pointer to the first overlapping pair (used to iterate over the active pairs)
BroadPhasePair* endOverlappingPairsPointer() const; // Return a pointer to the last overlapping pair (used to iterate over the active pairs)
void registerAddedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BroadPhasePair* addedActivePair)); // Register a callback function (using a function pointer) that will be called when a new overlapping pair is added in the pair manager
BodyPair* findPair(bodyindex id1, bodyindex id2) const; // Find a pair given two body IDs
BodyPair* beginOverlappingPairsPointer() const; // Return a pointer to the first overlapping pair (used to iterate over the active pairs)
BodyPair* endOverlappingPairsPointer() const; // Return a pointer to the last overlapping pair (used to iterate over the active pairs)
void registerAddedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BodyPair* addedActivePair)); // Register a callback function (using a function pointer) that will be called when a new overlapping pair is added in the pair manager
void unregisterAddedOverlappingPairCallback(); // Unregister the callback function that will be called when a new active pair is added in the pair manager
void registerRemovedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BroadPhasePair* removedActivePair)); // Register a callback function (using a function pointer) that will be called when an overlapping pair is removed from the pair manager
void registerRemovedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BodyPair* removedActivePair)); // Register a callback function (using a function pointer) that will be called when an overlapping pair is removed from the pair manager
void unregisterRemovedOverlappingPairCallback(); // Unregister a callback function that will be called when a active pair is removed from the pair manager
};
@ -110,7 +122,7 @@ inline uint PairManager::computeHashBodies(uint id1, uint id2) const {
}
// Return true if pair1 and pair2 are the same
inline bool PairManager::isDifferentPair(const BroadPhasePair& pair1, bodyindex pair2ID1, bodyindex pair2ID2) const {
inline bool PairManager::isDifferentPair(const BodyPair& pair1, bodyindex pair2ID1, bodyindex pair2ID2) const {
return (pair2ID1 != pair1.body1->getID() || pair2ID2 != pair1.body2->getID());
}
@ -161,7 +173,7 @@ inline int PairManager::computeHash32Bits(int key) const {
}
// Find a pair given two body IDs
inline BroadPhasePair* PairManager::findPair(bodyindex id1, bodyindex id2) const {
inline BodyPair* PairManager::findPair(bodyindex id1, bodyindex id2) const {
// Check if the hash table has been allocated yet
if (!hashTable) return 0;
@ -179,7 +191,7 @@ inline BroadPhasePair* PairManager::findPair(bodyindex id1, bodyindex id2) const
// Find a pair given two body IDs and an hash value
// This internal version is used to avoid computing multiple times in the
// caller method
inline BroadPhasePair* PairManager::findPairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue) const {
inline BodyPair* PairManager::findPairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue) const {
// Check if the hash table has been allocated yet
if (!hashTable) return 0;
@ -202,19 +214,19 @@ inline void PairManager::shrinkMemory() {
}
// Compute the offset of a given pair in the array of overlapping pairs
inline bodyindex PairManager::computePairOffset(const BroadPhasePair* pair) const {
return ((bodyindex)((size_t(pair) - size_t(overlappingPairs))) / sizeof(BroadPhasePair));
inline bodyindex PairManager::computePairOffset(const BodyPair* pair) const {
return ((bodyindex)((size_t(pair) - size_t(overlappingPairs))) / sizeof(BodyPair));
}
// Return a pointer to the first overlapping pair (used to iterate over the overlapping pairs) or
// returns 0 if there is no overlapping pairs.
inline BroadPhasePair* PairManager::beginOverlappingPairsPointer() const {
inline BodyPair* PairManager::beginOverlappingPairsPointer() const {
return &overlappingPairs[0];
}
// Return a pointer to the last overlapping pair (used to iterate over the overlapping pairs) or
// returns 0 if there is no overlapping pairs.
inline BroadPhasePair* PairManager::endOverlappingPairsPointer() const {
inline BodyPair* PairManager::endOverlappingPairsPointer() const {
if (nbOverlappingPairs > 0) {
return &overlappingPairs[nbOverlappingPairs-1];
}

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

@ -28,7 +28,6 @@
#include "Simplex.h"
#include "../../../constraint/Contact.h"
#include "../../../configuration.h"
#include "../../OverlappingPair.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
@ -87,7 +86,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, const Tr
Simplex simplex;
// Get the previous point V (last cached separating axis)
Vector3 v = currentOverlappingPair->getCachedSeparatingAxis();
Vector3 v = currentOverlappingPair->previousSeparatingAxis;
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@ -107,7 +106,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, const Tr
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
currentOverlappingPair->setCachedSeparatingAxis(v);
currentOverlappingPair->previousSeparatingAxis = v;
// No intersection, we return false
return false;

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

@ -29,7 +29,10 @@
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "../OverlappingPair.h"
#include "../broadphase/PairManager.h"
#include "../../memory/MemoryPool.h"
#include "../BroadPhasePair.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -46,20 +49,20 @@ namespace reactphysics3d {
class NarrowPhaseAlgorithm {
protected :
MemoryPool<ContactInfo>& memoryPoolContactInfos; // Reference to the memory pool for contact infos
OverlappingPair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision
BroadPhasePair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision
public :
NarrowPhaseAlgorithm(MemoryPool<ContactInfo>& memoryPool); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor
void setCurrentOverlappingPair(OverlappingPair* overlappingPair); // Set the current overlapping pair of bodies
void setCurrentOverlappingPair(BroadPhasePair* overlappingPair); // Set the current overlapping pair of bodies
virtual bool testCollision(const CollisionShape* collisionShape1, const Transform& transform1,
const CollisionShape* collisionShape2, const Transform& transform2,
ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
};
// Set the current overlapping pair of bodies
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(BroadPhasePair *overlappingPair) {
currentOverlappingPair = overlappingPair;
}

2
src/configuration.h
View File

@ -29,6 +29,7 @@
// Libraries
#include <limits>
#include <cfloat>
#include <utility>
#include "decimal.h"
// Windows platform
@ -47,6 +48,7 @@ namespace reactphysics3d {
typedef unsigned int uint;
typedef long unsigned int luint;
typedef short unsigned int bodyindex; // TODO : Replace whenever possible using the bodyindex type
typedef std::pair<bodyindex, bodyindex> bodyindexpair;
// Mathematical constants
const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits<reactphysics3d::decimal>::max(); // Minimun decimal value

0
src/collision/OverlappingPair.cpp → src/engine/OverlappingPair.cpp
View File

0
src/collision/OverlappingPair.h → src/engine/OverlappingPair.h
View File

0
src/collision/PersistentContactCache.cpp → src/engine/PersistentContactCache.cpp
View File

0
src/collision/PersistentContactCache.h → src/engine/PersistentContactCache.h
View File

59
src/engine/PhysicsWorld.cpp
View File

@ -40,6 +40,12 @@ PhysicsWorld::PhysicsWorld(const Vector3& gravity)
// Destructor
PhysicsWorld::~PhysicsWorld() {
// Delete the remaining overlapping pairs
for (map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator it=overlappingPairs.begin(); it != overlappingPairs.end(); it++) {
// Delete the overlapping pair
(*it).second->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject((*it).second);
}
}
// Create a rigid body into the physics world
@ -118,3 +124,56 @@ void PhysicsWorld::removeAllConstraints() {
constraints.clear();
}
// Notify the world about a new broad-phase overlapping pair
void PhysicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
// Get the pair of body index
std::pair<bodyindex, bodyindex> indexPair = addedPair->getBodiesIndexPair();
// Add the pair into the set of overlapping pairs (if not there yet)
OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(addedPair->body1, addedPair->body2, memoryPoolContacts);
std::pair<map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, newPair));
assert(check.second);
}
// Notify the world about a removed broad-phase overlapping pair
void PhysicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
// Get the pair of body index
std::pair<bodyindex, bodyindex> indexPair = removedPair->getBodiesIndexPair();
// Remove the overlapping pair from the memory pool
overlappingPairs[indexPair]->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(overlappingPairs[indexPair]);
overlappingPairs.erase(indexPair);
}
// Notify the world about a new narrow-phase contact
void PhysicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const ContactInfo* contactInfo) {
RigidBody* const rigidBody1 = dynamic_cast<RigidBody* const>(broadPhasePair->body1);
RigidBody* const rigidBody2 = dynamic_cast<RigidBody* const>(broadPhasePair->body2);
assert(rigidBody1);
assert(rigidBody2);
// Create a new contact
Contact* contact = new (memoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo);
assert(contact);
// Get the corresponding overlapping pair
pair<bodyindex, bodyindex> indexPair = broadPhasePair->getBodiesIndexPair();
OverlappingPair* overlappingPair = overlappingPairs[indexPair];
assert(overlappingPair);
// Add the contact to the contact cache of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add all the contacts in the contact cache of the two bodies
// to the set of constraints in the physics world
for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
addConstraint(overlappingPair->getContact(i));
}
}

30
src/engine/PhysicsWorld.h
View File

@ -32,6 +32,7 @@
#include <algorithm>
#include "../mathematics/mathematics.h"
#include "../body/CollisionBody.h"
#include "OverlappingPair.h"
#include "../collision/CollisionDetection.h"
#include "../constraint/Constraint.h"
#include "../constraint/Contact.h"
@ -54,11 +55,19 @@ class PhysicsWorld {
std::set<RigidBody*> rigidBodies; // All the rigid bodies of the physics world
std::vector<luint> freeRigidBodyIDs; // List of free ID for rigid bodies
std::vector<Constraint*> constraints; // List that contains all the current constraints
std::map<std::pair<bodyindex, bodyindex>, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
Vector3 gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on
bodyindex currentBodyID; // Current body ID
MemoryPool<RigidBody> memoryPoolRigidBodies; // Memory pool for rigid bodies memory allocation
MemoryPool<RigidBody> memoryPoolRigidBodies; // Memory pool for rigid bodies memory allocation
MemoryPool<OverlappingPair> memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs
MemoryPool<Contact> memoryPoolContacts; // Memory pool for the contacts
void notifyAddedOverlappingPair(const BroadPhasePair* addedPair); // Notify the world about a new broad-phase overlapping pair
void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); // Notify the world about a removed broad-phase overlapping pair
void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo); // Notify the world about a new narrow-phase contact
void updateOverlappingPair(const BroadPhasePair* pair); // Update the overlapping pair
public :
PhysicsWorld(const Vector3& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor
@ -80,8 +89,25 @@ class PhysicsWorld {
std::set<CollisionBody*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator(); // Return an iterator to the end of the rigid bodies of the physics world
// Friends
friend class CollisionDetection;
};
// Update the overlapping pair
inline void PhysicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
// Get the pair of body index
std::pair<bodyindex, bodyindex> indexPair = pair->getBodiesIndexPair();
// Get the corresponding overlapping pair
OverlappingPair* overlappingPair = overlappingPairs[indexPair];
// Update the contact cache of the overlapping pair
overlappingPair->update();
}
// Add a constraint into the physics world
inline void PhysicsWorld::addConstraint(Constraint* constraint) {
assert(constraint != 0);