Add the PairManager class
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@467 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
cd47b3e617
commit
63f887dc07
283
src/collision/broadphase/PairManager.cpp
Normal file
283
src/collision/broadphase/PairManager.cpp
Normal file
|
|
@ -0,0 +1,283 @@
|
|||
/********************************************************************************
|
||||
* 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 "PairManager.h"
|
||||
#include "../CollisionDetection.h"
|
||||
#include <cassert>
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor of PairManager
|
||||
PairManager::PairManager(CollisionDetection& collisionDetection) : collisionDetection(collisionDetection) {
|
||||
hashTable = 0;
|
||||
overlappingPairs = 0;
|
||||
offsetNextPair = 0;
|
||||
nbOverlappingPairs = 0;
|
||||
hashMask = 0;
|
||||
nbElementsHashTable = 0;
|
||||
}
|
||||
|
||||
// Destructor of PairManager
|
||||
PairManager::~PairManager() {
|
||||
|
||||
// Release the allocated memory
|
||||
free(offsetNextPair);
|
||||
free(overlappingPairs);
|
||||
free(hashTable);
|
||||
}
|
||||
|
||||
// Add a pair of bodies in the pair manager and returns a pointer to
|
||||
// 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(Body* body1, Body* body2) {
|
||||
|
||||
// Sort the bodies to have the body with smallest ID first
|
||||
sortBodiesUsingID(body1, body2);
|
||||
|
||||
// Get the bodies IDs
|
||||
luint id1 = body1->getID();
|
||||
luint id2 = body2->getID();
|
||||
|
||||
// Compute the hash value of the two bodies
|
||||
uint hashValue = computeHashBodies(id1, id2) & hashMask;
|
||||
|
||||
// Try to find the pair in the current overlapping pairs.
|
||||
BroadPhasePair* pair = findPairWithHashValue(id1, id2, hashValue);
|
||||
|
||||
// If the pair is already in the set of overlapping pairs
|
||||
if (pair) {
|
||||
// We only return a pointer to that pair
|
||||
return pair;
|
||||
}
|
||||
|
||||
// If we need to allocate more pairs in the set of overlapping pairs
|
||||
if (nbOverlappingPairs >= nbElementsHashTable) {
|
||||
// Increase the size of the hash table (always a power of two)
|
||||
nbElementsHashTable = computeNextPowerOfTwo(nbOverlappingPairs + 1);
|
||||
|
||||
// Compute the new hash mask with the new hash size
|
||||
hashMask = nbElementsHashTable - 1;
|
||||
|
||||
// Reallocate more pairs
|
||||
reallocatePairs();
|
||||
|
||||
// Compute the new hash value (using the new hash size and hash mask)
|
||||
hashValue = computeHashBodies(id1, id2) & hashMask;
|
||||
}
|
||||
|
||||
// Create the new overlapping pair
|
||||
BroadPhasePair* newPair = &overlappingPairs[nbOverlappingPairs];
|
||||
newPair->body1 = body1;
|
||||
newPair->body2 = body2;
|
||||
|
||||
// Put the new pair as the initial pair with this hash value
|
||||
offsetNextPair[nbOverlappingPairs] = hashTable[hashValue];
|
||||
hashTable[hashValue] = nbOverlappingPairs++;
|
||||
|
||||
// Notify the collision detection about this new overlapping pair
|
||||
collisionDetection.broadPhaseNotifyAddedOverlappingPair(newPair);
|
||||
|
||||
// Return a pointer to the new created pair
|
||||
return newPair;
|
||||
}
|
||||
|
||||
// Remove a pair of bodies from the pair manager. This method returns
|
||||
// true if the pair has been found and removed.
|
||||
bool PairManager::removePair(luint id1, luint id2) {
|
||||
|
||||
// Sort the bodies IDs
|
||||
sortIDs(id1, id2);
|
||||
|
||||
// Compute the hash value of the pair to remove
|
||||
const uint hashValue = computeHashBodies(id1, id2) & hashMask;
|
||||
|
||||
// Find the pair to remove
|
||||
const BroadPhasePair* pair = findPairWithHashValue(id1, id2, hashValue);
|
||||
|
||||
// If we have not found the pair
|
||||
if (!pair) {
|
||||
return false;
|
||||
}
|
||||
|
||||
assert(pair->body1->getID() == id1);
|
||||
assert(pair->body2->getID() == id2);
|
||||
|
||||
// Notify the collision detection about this removed overlapping pair
|
||||
collisionDetection.broadPhaseNotifyRemovedOverlappingPair(pair);
|
||||
|
||||
// Remove the pair from the set of overlapping pairs
|
||||
removePairWithHashValue(id1, id2, hashValue, computePairOffset(pair));
|
||||
|
||||
// Try to shrink the memory used by the pair manager
|
||||
shrinkMemory();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Internal method to remove a pair from the set of overlapping pair
|
||||
void PairManager::removePairWithHashValue(luint id1, luint id2, luint hashValue, luint indexPair) {
|
||||
|
||||
// Get the initial offset of the pairs with
|
||||
// the corresponding hash value
|
||||
luint offset = hashTable[hashValue];
|
||||
assert(offset != INVALID_OFFSET);
|
||||
|
||||
// Look for the pair in the set of overlapping pairs
|
||||
luint previousPair = INVALID_OFFSET;
|
||||
while(offset != indexPair) {
|
||||
previousPair = offset;
|
||||
offset = offsetNextPair[offset];
|
||||
}
|
||||
|
||||
// If the pair was the first one with this hash
|
||||
// value in the hash table
|
||||
if (previousPair == INVALID_OFFSET) {
|
||||
// Replace the pair to remove in the
|
||||
// hash table by the next one
|
||||
hashTable[hashValue] = offsetNextPair[indexPair];
|
||||
}
|
||||
else { // If the pair was not the first one
|
||||
// Replace the pair to remove in the
|
||||
// hash table by the next one
|
||||
assert(offsetNextPair[previousPair] == indexPair);
|
||||
offsetNextPair[previousPair] = offsetNextPair[indexPair];
|
||||
}
|
||||
|
||||
const luint indexLastPair = nbOverlappingPairs - 1;
|
||||
|
||||
// If the pair to remove is the last one in the list
|
||||
if (indexPair == indexLastPair) {
|
||||
|
||||
// We simply decrease the number of overlapping pairs
|
||||
nbOverlappingPairs--;
|
||||
}
|
||||
else { // If the pair to remove is in the middle of the list
|
||||
|
||||
// Now, we want to move the last pair into the location that is
|
||||
// now free because of the pair we want to remove
|
||||
|
||||
// Get the last pair
|
||||
const BroadPhasePair* lastPair = &overlappingPairs[indexLastPair];
|
||||
const uint lastPairHashValue = computeHashBodies(lastPair->body1->getID(), lastPair->body2->getID()) & hashMask;
|
||||
|
||||
// Compute the initial offset of the last pair
|
||||
luint offset = hashTable[lastPairHashValue];
|
||||
assert(offset != INVALID_OFFSET);
|
||||
|
||||
// Go through the pairs with the same hash value
|
||||
// and find the offset of the last pair
|
||||
luint previous = INVALID_OFFSET;
|
||||
while(offset != indexLastPair) {
|
||||
previous = offset;
|
||||
offset = offsetNextPair[offset];
|
||||
}
|
||||
|
||||
// If the last pair is not the first one with this hash value
|
||||
if (previous != INVALID_OFFSET) {
|
||||
|
||||
// Remove the offset of the last pair in the "nextOffset" array
|
||||
assert(offsetNextPair[previous] == indexLastPair);
|
||||
offsetNextPair[previous] = offsetNextPair[indexLastPair];
|
||||
}
|
||||
else { // If the last pair is the first offset with this hash value
|
||||
|
||||
// Remove the offset of the last pair in the "nextOffset" array
|
||||
hashTable[lastPairHashValue] = offsetNextPair[indexLastPair];
|
||||
}
|
||||
|
||||
// Replace the pair to remove by the last pair in
|
||||
// the overlapping pairs array
|
||||
overlappingPairs[indexPair] = overlappingPairs[indexLastPair];
|
||||
offsetNextPair[indexPair] = hashTable[lastPairHashValue];
|
||||
hashTable[lastPairHashValue] = indexPair;
|
||||
|
||||
nbOverlappingPairs--;
|
||||
}
|
||||
}
|
||||
|
||||
// Look for a pair in the set of overlapping pairs
|
||||
BroadPhasePair* PairManager::lookForAPair(luint id1, luint id2, luint hashValue) const {
|
||||
|
||||
// Look for the pair in the set of overlapping pairs
|
||||
luint offset = hashTable[hashValue];
|
||||
while (offset != INVALID_OFFSET && isDifferentPair(overlappingPairs[offset], id1, id2)) {
|
||||
offset = offsetNextPair[offset];
|
||||
}
|
||||
|
||||
// If the pair has not been found in the overlapping pairs
|
||||
if (offset == INVALID_OFFSET) {
|
||||
// Return null
|
||||
return 0;
|
||||
}
|
||||
|
||||
assert(offset < nbOverlappingPairs);
|
||||
|
||||
// The pair has been found in the set of overlapping pairs, then
|
||||
// we return a pointer to it
|
||||
return &overlappingPairs[offset];
|
||||
}
|
||||
|
||||
// Reallocate more pairs
|
||||
void PairManager::reallocatePairs() {
|
||||
|
||||
// Reallocate the hash table and initialize it
|
||||
free(hashTable);
|
||||
hashTable = (luint*) malloc(nbElementsHashTable * sizeof(luint));
|
||||
assert(hashTable);
|
||||
for (luint i=0; i<nbElementsHashTable; i++) {
|
||||
hashTable[i] = INVALID_OFFSET;
|
||||
}
|
||||
|
||||
// Reallocate the overlapping pairs
|
||||
BroadPhasePair* newOverlappingPairs = (BroadPhasePair*) malloc(nbElementsHashTable * sizeof(BroadPhasePair));
|
||||
luint* newNextOffset = (luint*) malloc(nbElementsHashTable * sizeof(luint));
|
||||
|
||||
assert(newOverlappingPairs);
|
||||
assert(newNextOffset);
|
||||
|
||||
// If there is already some overlapping pairs
|
||||
if (nbOverlappingPairs) {
|
||||
// Copy the pairs to the new place
|
||||
memcpy(newOverlappingPairs, overlappingPairs, nbOverlappingPairs * sizeof(BroadPhasePair));
|
||||
}
|
||||
|
||||
// Recompute the hash table with the new hash values
|
||||
for (luint i=0; i<nbOverlappingPairs; i++) {
|
||||
const uint newHashValue = computeHashBodies(overlappingPairs[i].body1->getID(), overlappingPairs[i].body2->getID()) & hashMask;
|
||||
newNextOffset[i] = hashTable[newHashValue];
|
||||
hashTable[newHashValue] = i;
|
||||
}
|
||||
|
||||
// Delete the old pairs
|
||||
free(offsetNextPair);
|
||||
free(overlappingPairs);
|
||||
|
||||
// Replace by the new data
|
||||
overlappingPairs = newOverlappingPairs;
|
||||
offsetNextPair = newNextOffset;
|
||||
}
|
||||
|
||||
231
src/collision/broadphase/PairManager.h
Normal file
231
src/collision/broadphase/PairManager.h
Normal file
|
|
@ -0,0 +1,231 @@
|
|||
/********************************************************************************
|
||||
* 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 PAIR_MANAGER_H
|
||||
#define PAIR_MANAGER_H
|
||||
|
||||
// Libraries
|
||||
#include "../../body/Body.h"
|
||||
|
||||
|
||||
// Namespace ReactPhysics3D
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Declaration
|
||||
class CollisionDetection;
|
||||
|
||||
// Structure BroadPhasePair that represents a pair of bodies
|
||||
// during the broad-phase collision detection
|
||||
struct BroadPhasePair {
|
||||
public:
|
||||
Body* body1; // Pointer to the first body
|
||||
Body* body2; // Pointer to the second body
|
||||
};
|
||||
|
||||
// TODO : Change and use uint instead of luint here
|
||||
|
||||
/* --------------------------------------------------------------------
|
||||
Class PairManager :
|
||||
This class is a data-structure contains the pairs of bodies that
|
||||
are overlapping during the broad-phase collision detection.
|
||||
This class implements the pair manager described by Pierre Terdiman
|
||||
in www.codercorner.com/SAP.pdf.
|
||||
--------------------------------------------------------------------
|
||||
*/
|
||||
class PairManager {
|
||||
private :
|
||||
luint nbElementsHashTable; // Number of elements in the hash table
|
||||
uint hashMask; // Hash mask for the hash function
|
||||
luint nbOverlappingPairs; // Number of overlapping pairs
|
||||
luint* hashTable; // Hash table that contains the offset of the first pair of the list of
|
||||
// pairs with the same hash value in the "overlappingPairs" array
|
||||
luint* 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
|
||||
static const luint INVALID_OFFSET = 0xffffffff; // Invalid ID
|
||||
CollisionDetection& collisionDetection; // Reference to the collision detection
|
||||
|
||||
void sortBodiesUsingID(Body*& body1, Body*& body2) const; // Sort the bodies according to their IDs (smallest ID first)
|
||||
void sortIDs(luint& id1, luint& id2) const; // Sort the IDs (smallest ID first)
|
||||
bool isDifferentPair(const BroadPhasePair& pair1, luint pair2ID1,
|
||||
luint 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
|
||||
luint computePairOffset(const BroadPhasePair* pair) const; // Compute the offset of a given pair
|
||||
BroadPhasePair* lookForAPair(luint id1, luint id2,
|
||||
luint hashValue) const; // Look for a pair in the set of overlapping pairs
|
||||
BroadPhasePair* findPairWithHashValue(luint id1, luint id2,
|
||||
luint hashValue) const; // Find a pair given two body IDs and an hash value
|
||||
void removePairWithHashValue(luint id1, luint id2, luint hashValue,
|
||||
luint indexPair); // Remove a pair from the set of active pair
|
||||
public :
|
||||
PairManager(CollisionDetection& collisionDetection); // Constructor
|
||||
~PairManager(); // Destructor
|
||||
|
||||
uint getNbOverlappingPairs() const; // Return the number of active pairs
|
||||
BroadPhasePair* addPair(Body* body1, Body* body2); // Add a pair of bodies in the pair manager
|
||||
bool removePair(luint id1, luint id2); // Remove a pair of bodies from the pair manager
|
||||
BroadPhasePair* findPair(luint id1, luint 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
|
||||
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 unregisterRemovedOverlappingPairCallback(); // Unregister a callback function that will be called when a active pair is removed from the pair manager
|
||||
};
|
||||
|
||||
// Return the number of overlapping pairs
|
||||
inline uint PairManager::getNbOverlappingPairs() const {
|
||||
return nbOverlappingPairs;
|
||||
}
|
||||
|
||||
// Compute the hash value of two bodies
|
||||
inline uint PairManager::computeHashBodies(uint id1, uint id2) const {
|
||||
return computeHash32Bits(id1 | (id2 << 16));
|
||||
}
|
||||
|
||||
// Return true if pair1 and pair2 are the same
|
||||
inline bool PairManager::isDifferentPair(const BroadPhasePair& pair1, luint pair2ID1, luint pair2ID2) const {
|
||||
return (pair2ID1 != pair1.body1->getID() || pair2ID2 != pair1.body2->getID());
|
||||
}
|
||||
|
||||
// Return the next power of two of a 32bits integer using a SWAR algorithm
|
||||
// TODO : Add documentation
|
||||
inline luint PairManager::computeNextPowerOfTwo(luint number) const {
|
||||
number |= (number >> 1);
|
||||
number |= (number >> 2);
|
||||
number |= (number >> 4);
|
||||
number |= (number >> 8);
|
||||
number |= (number >> 16);
|
||||
return number+1;
|
||||
}
|
||||
|
||||
// Sort the bodies according to their IDs (smallest ID first)
|
||||
inline void PairManager::sortBodiesUsingID(Body*& body1, Body*& body2) const {
|
||||
|
||||
// If the ID of body1 is larger than the ID of body 2
|
||||
if (body1->getID() > body2->getID()) {
|
||||
|
||||
// Swap the two bodies pointers
|
||||
Body* temp = body2;
|
||||
body2 = body1;
|
||||
body1 = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Sort the IDs (smallest ID first)
|
||||
inline void PairManager::sortIDs(luint& id1, luint& id2) const {
|
||||
if (id1 > id2) {
|
||||
luint temp = id2;
|
||||
id2 = id1;
|
||||
id1 = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// This method returns an hash value for a 32 bits key
|
||||
// using Thomas Wang's hash technique.
|
||||
// TODO : Add documentation here
|
||||
inline int PairManager::computeHash32Bits(int key) const {
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
|
||||
// Find a pair given two body IDs
|
||||
inline BroadPhasePair* PairManager::findPair(luint id1, luint id2) const {
|
||||
|
||||
// Check if the hash table has been allocated yet
|
||||
if (!hashTable) return 0;
|
||||
|
||||
// Sort the IDs
|
||||
sortIDs(id1, id2);
|
||||
|
||||
// Compute the hash value of the pair to find
|
||||
uint hashValue = computeHashBodies(id1, id2) & hashMask;
|
||||
|
||||
// Look for the pair in the set of overlapping pairs
|
||||
lookForAPair(id1, id2, hashValue);
|
||||
}
|
||||
|
||||
// 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(luint id1, luint id2, luint hashValue) const {
|
||||
|
||||
// Check if the hash table has been allocated yet
|
||||
if (!hashTable) return 0;
|
||||
|
||||
// Look for the pair in the set of overlapping pairs
|
||||
return lookForAPair(id1, id2, hashValue);
|
||||
}
|
||||
|
||||
// Try to reduce the allocated memory by the pair manager
|
||||
inline void PairManager::shrinkMemory() {
|
||||
|
||||
// Check if the allocated memory can be reduced
|
||||
const luint correctNbElementsHashTable = computeNextPowerOfTwo(nbOverlappingPairs);
|
||||
if (nbElementsHashTable == correctNbElementsHashTable) return;
|
||||
|
||||
// Reduce the allocated memory
|
||||
nbElementsHashTable = correctNbElementsHashTable;
|
||||
hashMask = nbElementsHashTable - 1;
|
||||
reallocatePairs();
|
||||
}
|
||||
|
||||
// Compute the offset of a given pair in the array of overlapping pairs
|
||||
inline luint PairManager::computePairOffset(const BroadPhasePair* pair) const {
|
||||
return ((luint)((size_t(pair) - size_t(overlappingPairs))) / sizeof(BroadPhasePair));
|
||||
}
|
||||
|
||||
// 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 {
|
||||
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 {
|
||||
if (nbOverlappingPairs > 0) {
|
||||
return &overlappingPairs[nbOverlappingPairs-1];
|
||||
}
|
||||
else {
|
||||
return &overlappingPairs[0];
|
||||
}
|
||||
}
|
||||
|
||||
} // End of reactphysics3d namespace
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user