optimized the island management. It was unoptimized, and becomes a bottleneck for large amounts of objects.

Bullet/CollisionDispatch/SimulationIslandManager.cpp

@@ -5,6 +5,8 @@
 #include "CollisionDispatch/CollisionObject.h"
 #include "CollisionDispatch/CollisionWorld.h"
 
+#include <stdio.h>
+#include <algorithm>
 
 
 SimulationIslandManager::SimulationIslandManager()
@@ -105,7 +107,23 @@ void	SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorl
 	}
 }
 
+inline	int	getIslandId(const PersistentManifold* lhs)
+{
+	int islandId;
+	const CollisionObject* rcolObj0 = static_cast<const CollisionObject*>(lhs->GetBody0());
+	const CollisionObject* rcolObj1 = static_cast<const CollisionObject*>(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<uf.getNumElements();islandId++)
+	//update the sleeping state for bodies, if all are sleeping
+	for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
 	{
-		if (uf.isRoot(islandId))
+		int islandId = GetUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (GetUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
 		{
+		}
 
-			std::vector<PersistentManifold*>  islandmanifold;
+		//int numSleeping = 0;
-			
-			//int numSleeping = 0;
 
-			bool allSleeping = true;
+		bool allSleeping = true;
 
-			int i;
+		int idx;
-			for (i=0;i<numBodies;i++)
+		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+		{
+			int i = GetUnionFind().getElement(idx).m_sz;
+
+			CollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
 			{
-				CollisionObject* colObj0 = collisionObjects[i];
+				printf("error in island management\n");
-			
-				if (colObj0->m_islandTag1 == islandId)
-				{
-				
-					if (colObj0->GetActivationState()== ACTIVE_TAG)
-					{
-						allSleeping = false;
-					}
-					if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
-					{
-						allSleeping = false;
-					}
-				}
 			}
 
-			
+			assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
-			
+			if (colObj0->m_islandTag1 == islandId)
-			if (allSleeping)
 			{
-				int i;
+				if (colObj0->GetActivationState()== ACTIVE_TAG)
-				for (i=0;i<numBodies;i++)
 				{
-					CollisionObject* colObj0 = collisionObjects[i];
+					allSleeping = false;
-					if (colObj0->m_islandTag1 == islandId)
+				}
+				if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+		
+		if (allSleeping)
+		{
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = GetUnionFind().getElement(idx).m_sz;
+				CollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->m_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;idx<endIslandIndex;idx++)
+			{
+				int i = GetUnionFind().getElement(idx).m_sz;
+
+				CollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->m_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;i<numBodies;i++)
-				{
-					CollisionObject* colObj0 = collisionObjects[i];
-					if (colObj0->m_islandTag1 == islandId)
-					{
-						if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
-						{
-							colObj0->SetActivationState( WANTS_DEACTIVATION);
-						}
-					}
-				}
-
-				for (i=0;i<dispatcher->GetNumManifolds();i++)
-				{
-					 PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
-					 
-					 //filtering for response
-
-					 CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
-					 CollisionObject* colObj1 = static_cast<CollisionObject*>(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<PersistentManifold*>  islandmanifold;
+	int i;
+	int numManifolds = dispatcher->GetNumManifolds();
+	islandmanifold.reserve(numManifolds);
+
+	for (i=0;i<numManifolds ;i++)
+	{
+		 PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
+		 
+		 CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
+		 CollisionObject* colObj1 = static_cast<CollisionObject*>(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;startManifoldIndex<numManifolds;startManifoldIndex = endManifoldIndex)
+	{
+		int islandId = getIslandId(islandmanifold[startManifoldIndex]);
+		for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
+		{
+		}
+		/// Process the actual simulation, only if not sleeping/deactivated
+		int numIslandManifolds = endManifoldIndex-startManifoldIndex;
+		if (numIslandManifolds)
+		{
+			callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds);
+		}
+	}
 }

Bullet/CollisionDispatch/UnionFind.cpp

@@ -15,28 +15,9 @@ subject to the following restrictions:
 
 #include "UnionFind.h"
 #include <assert.h>
+#include <algorithm>
 
 
-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_elements(0),
-m_sz(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_elements.resize(N);// = new Element[N]; 
-		m_sz = new int[N];
 	}
 }
 void	UnionFind::Free()
@@ -68,8 +47,7 @@ void	UnionFind::Free()
 	if (m_N)
 	{
 		m_N=0;
-		delete m_id;
+		m_elements.clear();
-		delete m_sz;
 	}
 }
 
@@ -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; 
 	} 
 }
 
-
+bool UnionFindElementSortPredicate(const Element& lhs, const Element& rhs)
-int UnionFind ::find(int p, int q)
-{ 
-	return (find(p) == find(q)); 
-}
-
-void UnionFind ::unite(int p, int q)
 {
-	int i = find(p), j = find(q);
+	return lhs.m_id < rhs.m_id;
-	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]; 
-	}
 }
+
+
+///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<numElements;i++)
+	{
+		m_elements[i].m_id = find(i);
+		m_elements[i].m_sz = i;
+	}
+	
+	 // Sort the vector using predicate and std::sort
+	  std::sort(m_elements.begin(), m_elements.end(), UnionFindElementSortPredicate);
+
+}
+

Bullet/CollisionDispatch/UnionFind.h

@@ -16,22 +16,32 @@ subject to the following restrictions:
 #ifndef UNION_FIND_H
 #define UNION_FIND_H
 
+#include <vector>
+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;
+		std::vector<Element>	m_elements;
-		int*	m_sz;
 		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);
+	  Element&	getElement(int index)
-      void unite(int p, int q);
+	  {
-
+		  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; 
+		}
+
+
   };