diff --git a/Bullet/CollisionDispatch/SimulationIslandManager.cpp b/Bullet/CollisionDispatch/SimulationIslandManager.cpp index 455d9f50c..02a70eb49 100644 --- a/Bullet/CollisionDispatch/SimulationIslandManager.cpp +++ b/Bullet/CollisionDispatch/SimulationIslandManager.cpp @@ -5,6 +5,8 @@ #include "CollisionDispatch/CollisionObject.h" #include "CollisionDispatch/CollisionWorld.h" +#include +#include SimulationIslandManager::SimulationIslandManager() @@ -105,7 +107,23 @@ void SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorl } } +inline int getIslandId(const PersistentManifold* lhs) +{ + int islandId; + const CollisionObject* rcolObj0 = static_cast(lhs->GetBody0()); + const CollisionObject* rcolObj1 = static_cast(lhs->GetBody1()); + islandId= rcolObj0->m_islandTag1>=0?rcolObj0->m_islandTag1:rcolObj1->m_islandTag1; + return islandId; +} + +bool PersistentManifoldSortPredicate(const PersistentManifold* lhs, const PersistentManifold* rhs) +{ + int rIslandId0,lIslandId0; + rIslandId0 = getIslandId(rhs); + lIslandId0 = getIslandId(lhs); + return lIslandId0 < rIslandId0; +} // @@ -114,101 +132,143 @@ void SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorl void SimulationIslandManager::BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback) { + int numBodies = collisionObjects.size(); - //first calculate the number of islands, and iterate over the islands id's + //we are going to sort the unionfind array, and store the element id in the size + //afterwards, we clean unionfind, to make sure no-one uses it anymore + + GetUnionFind().sortIslands(); + int numElem = GetUnionFind().getNumElements(); - const UnionFind& uf = this->GetUnionFind(); + int startIslandIndex=0,endIslandIndex=1; - for (int islandId=0;islandId islandmanifold; - - //int numSleeping = 0; + //int numSleeping = 0; - bool allSleeping = true; + bool allSleeping = true; - int i; - for (i=0;im_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) { - CollisionObject* colObj0 = collisionObjects[i]; - - if (colObj0->m_islandTag1 == islandId) - { - - if (colObj0->GetActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } + printf("error in island management\n"); } - - - if (allSleeping) + assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + if (colObj0->m_islandTag1 == islandId) { - int i; - for (i=0;iGetActivationState()== ACTIVE_TAG) { - CollisionObject* colObj0 = collisionObjects[i]; - if (colObj0->m_islandTag1 == islandId) + allSleeping = false; + } + if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; + } + } + } + + if (allSleeping) + { + int idx; + for (idx=startIslandIndex;idxm_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + { + printf("error in island management\n"); + } + + assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + + if (colObj0->m_islandTag1 == islandId) + { + colObj0->SetActivationState( ISLAND_SLEEPING ); + } + } + } else + { + + int idx; + for (idx=startIslandIndex;idxm_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + { + printf("error in island management\n"); + } + + assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + + if (colObj0->m_islandTag1 == islandId) + { + if ( colObj0->GetActivationState() == ISLAND_SLEEPING) { - colObj0->SetActivationState( ISLAND_SLEEPING ); + colObj0->SetActivationState( WANTS_DEACTIVATION); } } - - - } else - { - - int i; - for (i=0;im_islandTag1 == islandId) - { - if ( colObj0->GetActivationState() == ISLAND_SLEEPING) - { - colObj0->SetActivationState( WANTS_DEACTIVATION); - } - } - } - - for (i=0;iGetNumManifolds();i++) - { - PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i); - - //filtering for response - - CollisionObject* colObj0 = static_cast(manifold->GetBody0()); - CollisionObject* colObj1 = static_cast(manifold->GetBody1()); - assert(colObj0); - assert(colObj1); - { - if (((colObj0)->m_islandTag1 == (islandId)) || - ((colObj1)->m_islandTag1 == (islandId))) - { - if (dispatcher->NeedsResponse(*colObj0,*colObj1)) - islandmanifold.push_back(manifold); - } - } - } - - - /// Process the actual simulation, only if not sleeping/deactivated - if (islandmanifold.size()) - { - callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); - } - } } } + + std::vector islandmanifold; + int i; + int numManifolds = dispatcher->GetNumManifolds(); + islandmanifold.reserve(numManifolds); + + for (i=0;iGetManifoldByIndexInternal(i); + + CollisionObject* colObj0 = static_cast(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast(manifold->GetBody1()); + + //todo: check sleeping conditions! + if (((colObj0) && colObj0->GetActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING)) + { + //filtering for response + if (dispatcher->NeedsResponse(*colObj0,*colObj1)) + islandmanifold.push_back(manifold); + } + } + + + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + std::sort(islandmanifold.begin(), islandmanifold.end(), PersistentManifoldSortPredicate); + + //now process all active islands (sets of manifolds for now) + + int startManifoldIndex = 0; + int endManifoldIndex = 1; + + for (startManifoldIndex=0;startManifoldIndexProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds); + } + } } diff --git a/Bullet/CollisionDispatch/UnionFind.cpp b/Bullet/CollisionDispatch/UnionFind.cpp index 43c87bb4f..e099408db 100644 --- a/Bullet/CollisionDispatch/UnionFind.cpp +++ b/Bullet/CollisionDispatch/UnionFind.cpp @@ -15,28 +15,9 @@ subject to the following restrictions: #include "UnionFind.h" #include +#include -int UnionFind::find(int x) -{ - assert(x < m_N); - assert(x >= 0); - - while (x != m_id[x]) - { -//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically -#define USE_PATH_COMPRESSION 1 -#ifdef USE_PATH_COMPRESSION - // - m_id[x] = m_id[m_id[x]]; -#endif // - x = m_id[x]; - assert(x < m_N); - assert(x >= 0); - - } - return x; -} UnionFind::~UnionFind() { @@ -45,8 +26,7 @@ UnionFind::~UnionFind() } UnionFind::UnionFind() -:m_id(0), -m_sz(0), +:m_elements(0), m_N(0) { @@ -56,11 +36,10 @@ void UnionFind::Allocate(int N) { if (m_N < N) { - Free(); + //Free(); //not necessary with stl vectors m_N = N; - m_id = new int[N]; - m_sz = new int[N]; + m_elements.resize(N);// = new Element[N]; } } void UnionFind::Free() @@ -68,8 +47,7 @@ void UnionFind::Free() if (m_N) { m_N=0; - delete m_id; - delete m_sz; + m_elements.clear(); } } @@ -80,29 +58,32 @@ void UnionFind::reset(int N) for (int i = 0; i < m_N; i++) { - m_id[i] = i; m_sz[i] = 1; + m_elements[i].m_id = i; m_elements[i].m_sz = 1; } } - -int UnionFind ::find(int p, int q) -{ - return (find(p) == find(q)); -} - -void UnionFind ::unite(int p, int q) +bool UnionFindElementSortPredicate(const Element& lhs, const Element& rhs) { - int i = find(p), j = find(q); - if (i == j) - return; - - //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) - if (m_sz[i] < m_sz[j]) - { - m_id[i] = j; m_sz[j] += m_sz[i]; - } - else - { - m_id[j] = i; m_sz[i] += m_sz[j]; - } + return lhs.m_id < rhs.m_id; } + + +///this is a special operation, destroying the content of UnionFind. +///it sorts the elements, based on island id, in order to make it easy to iterate over islands +void UnionFind::sortIslands() +{ + + //first store the original body index, and islandId + int numElements = m_elements.size(); + + for (int i=0;i +struct Element +{ + int m_id; + int m_sz; +}; + ///UnionFind calculates connected subsets // Implements weighted Quick Union with path compression // optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable) class UnionFind { private: - int* m_id; - int* m_sz; + std::vector m_elements; int m_N; public: - int find(int x); UnionFind(); ~UnionFind(); + + //this is a special operation, destroying the content of UnionFind. + //it sorts the elements, based on island id, in order to make it easy to iterate over islands + void sortIslands(); + void reset(int N); inline int getNumElements() const @@ -40,15 +50,68 @@ class UnionFind } inline bool isRoot(int x) const { - return (x == m_id[x]); + return (x == m_elements[x].m_id); } - int find(int p, int q); - void unite(int p, int q); - + Element& getElement(int index) + { + return m_elements[index]; + } + const Element& getElement(int index) const + { + return m_elements[index]; + } + void Allocate(int N); void Free(); + + + + int find(int p, int q) + { + return (find(p) == find(q)); + } + + void unite(int p, int q) + { + int i = find(p), j = find(q); + if (i == j) + return; + + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + if (m_elements[i].m_sz < m_elements[j].m_sz) + { + m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; + } + else + { + m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; + } + } + + int find(int x) + { + //assert(x < m_N); + //assert(x >= 0); + + while (x != m_elements[x].m_id) + { + //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + #define USE_PATH_COMPRESSION 1 + #ifdef USE_PATH_COMPRESSION + // + m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; + #endif // + x = m_elements[x].m_id; + //assert(x < m_N); + //assert(x >= 0); + + } + return x; + } + + };