diff --git a/Bullet/CollisionDispatch/SimulationIslandManager.cpp b/Bullet/CollisionDispatch/SimulationIslandManager.cpp index 0850192dc..0adea311b 100644 --- a/Bullet/CollisionDispatch/SimulationIslandManager.cpp +++ b/Bullet/CollisionDispatch/SimulationIslandManager.cpp @@ -116,88 +116,96 @@ void SimulationIslandManager::BuildAndProcessIslands(Dispatcher* dispatcher,Coll int numBodies = collisionObjects.size(); - for (int islandId=0;islandIdGetUnionFind(); + + for (int islandId=0;islandId islandmanifold; - - //int numSleeping = 0; - - bool allSleeping = true; - - int i; - for (i=0;im_islandTag1 == islandId) - { + + std::vector islandmanifold; - if (colObj0->GetActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } - } + //int numSleeping = 0; - - 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()); - { - if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || - ((colObj1) && (colObj1)->m_islandTag1 == (islandId))) - { - - if (dispatcher->NeedsResponse(*colObj0,*colObj1)) - islandmanifold.push_back(manifold); - } - } - } - if (allSleeping) - { - int i; - for (i=0;im_islandTag1 == islandId) - { - colObj0->SetActivationState( ISLAND_SLEEPING ); - } - } - - - } else - { + bool allSleeping = true; int i; for (i=0;im_islandTag1 == islandId) { - if ( colObj0->GetActivationState() == ISLAND_SLEEPING) + + if (colObj0->GetActivationState()== ACTIVE_TAG) { - colObj0->SetActivationState( WANTS_DEACTIVATION); + allSleeping = false; + } + if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; } } } - /// Process the actual simulation, only if not sleeping/deactivated - if (islandmanifold.size()) + + for (i=0;iGetNumManifolds();i++) { - callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); - } + 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); + } + } + } + if (allSleeping) + { + int i; + for (i=0;im_islandTag1 == islandId) + { + colObj0->SetActivationState( ISLAND_SLEEPING ); + } + } + + + } else + { + + int i; + for (i=0;im_islandTag1 == islandId) + { + if ( colObj0->GetActivationState() == ISLAND_SLEEPING) + { + colObj0->SetActivationState( WANTS_DEACTIVATION); + } + } + } + + /// Process the actual simulation, only if not sleeping/deactivated + if (islandmanifold.size()) + { + callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); + } + + } } } } diff --git a/Bullet/CollisionDispatch/UnionFind.cpp b/Bullet/CollisionDispatch/UnionFind.cpp index 8f456ea17..43c87bb4f 100644 --- a/Bullet/CollisionDispatch/UnionFind.cpp +++ b/Bullet/CollisionDispatch/UnionFind.cpp @@ -24,6 +24,12 @@ int UnionFind::find(int x) 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); @@ -89,6 +95,8 @@ void UnionFind ::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_sz[i] < m_sz[j]) { m_id[i] = j; m_sz[j] += m_sz[i]; diff --git a/Bullet/CollisionDispatch/UnionFind.h b/Bullet/CollisionDispatch/UnionFind.h index f7e9feb6e..8705e5791 100644 --- a/Bullet/CollisionDispatch/UnionFind.h +++ b/Bullet/CollisionDispatch/UnionFind.h @@ -17,6 +17,8 @@ subject to the following restrictions: #define UNION_FIND_H ///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: @@ -32,6 +34,15 @@ class UnionFind void reset(int N); + inline int getNumElements() const + { + return m_N; + } + inline bool isRoot(int x) const + { + return (x == m_id[x]); + } + int find(int p, int q); void unite(int p, int q);