From 6f7601966dfc2ae73eeb0e2a32a98c7c24d46261 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Wed, 3 May 2017 11:23:01 -0700
Subject: [PATCH] fix issue ,don't iterate over [0..numBodyHandles], would fail
 once we allow 'removeBody'...

---
 .../PhysicsServerCommandProcessor.cpp         |  7 ++++---
 src/Bullet3Common/b3ResizablePool.h           | 20 ++++++++++++++++++-
 2 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index a120731b0..bef003805 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -2793,11 +2793,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 				{
 					BT_PROFILE("CMD_SYNC_BODY_INFO");
 
-					int numHandles = m_data->m_bodyHandles.getNumHandles();
+					b3AlignedObjectArray<int> usedHandles;
+					m_data->m_bodyHandles.getUsedHandles(usedHandles);
 					int actualNumBodies = 0;
-					for (int i=0;i<numHandles;i++)
+					for (int i=0;i<usedHandles.size();i++)
 					{
-						InteralBodyData* body = m_data->m_bodyHandles.getHandle(i);
+						InteralBodyData* body = m_data->m_bodyHandles.getHandle(usedHandles[i]);
 						if (body && (body->m_multiBody || body->m_rigidBody))
 						{
 							serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = i;
diff --git a/src/Bullet3Common/b3ResizablePool.h b/src/Bullet3Common/b3ResizablePool.h
index 4df56edde..ec5495948 100644
--- a/src/Bullet3Common/b3ResizablePool.h
+++ b/src/Bullet3Common/b3ResizablePool.h
@@ -4,6 +4,12 @@
 
 #include "Bullet3Common/b3AlignedObjectArray.h"
 
+enum 
+{
+	B3_POOL_HANDLE_TERMINAL_FREE=-1,
+	B3_POOL_HANDLE_TERMINAL_USED =-2
+};
+
 template <typename U>
 struct b3PoolBodyHandle : public U
 {
@@ -45,6 +51,18 @@ public:
 		return m_bodyHandles.size();
 	}
 
+	void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
+	{
+
+		for (int i=0;i<m_bodyHandles.size();i++)
+		{
+			if (m_bodyHandles[i].GetNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+			{
+				usedHandles.push_back(i);
+			}
+		}
+	}
+
 	T* getHandle(int handle)
 	{
 		btAssert(handle>=0);
@@ -108,7 +126,7 @@ public:
 
 			getHandle(handle)->SetNextFree(m_firstFreeHandle);
 		}
-
+		getHandle(handle)->SetNextFree(B3_POOL_HANDLE_TERMINAL_USED);
 
 		return handle;
 	}