Fixed deleting objects procedure for btSimpleBroadphase

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -55,6 +55,7 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o
 	m_maxHandles = maxProxies;
 	m_numHandles = 0;
 	m_firstFreeHandle = 0;
+	m_LastHandleIndex = -1;
 	
 
 	{
@@ -153,9 +154,13 @@ void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbM
 
 void	btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
 {
-	for (int i=0;i<m_numHandles;i++)
+	for (int i=0; i <= m_LastHandleIndex; i++)
 	{
 		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+		if(!proxy->m_clientObject)
+		{
+			continue;
+		}
 		rayCallback.process(proxy);
 	}
 }
@@ -190,18 +195,25 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
 	//first check for new overlapping pairs
 	int i,j;
-
 	if (m_numHandles >= 0)
 	{
-
+		int new_largest_index = -1;
-		for (i=0;i<m_numHandles;i++)
+		for (i=0; i <= m_LastHandleIndex; i++)
 		{
 			btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
-
+			if(!proxy0->m_clientObject)
-			for (j=i+1;j<m_numHandles;j++)
+			{
+				continue;
+			}
+			new_largest_index = i;
+			for (j=i+1; j <= m_LastHandleIndex; j++)
 			{
 				btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
 				btAssert(proxy0 != proxy1);
+				if(!proxy1->m_clientObject)
+				{
+					continue;
+				}
 
 				btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 				btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
@@ -225,6 +237,8 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 			}
 		}
 
+		m_LastHandleIndex = new_largest_index;
+
 		if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
 		{
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -53,6 +53,7 @@ protected:
 
 	int		m_numHandles;						// number of active handles
 	int		m_maxHandles;						// max number of handles
+	int		m_LastHandleIndex;							
 	
 	btSimpleBroadphaseProxy* m_pHandles;						// handles pool
 
@@ -65,6 +66,10 @@ protected:
 		int freeHandle = m_firstFreeHandle;
 		m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
 		m_numHandles++;
+		if(freeHandle > m_LastHandleIndex)
+		{
+			m_LastHandleIndex = freeHandle;
+		}
 		return freeHandle;
 	}
 
@@ -72,10 +77,15 @@ protected:
 	{
 		int handle = int(proxy-m_pHandles);
 		btAssert(handle >= 0 && handle < m_maxHandles);
-
+		if(handle == m_LastHandleIndex)
+		{
+			m_LastHandleIndex--;
+		}
 		proxy->SetNextFree(m_firstFreeHandle);
 		m_firstFreeHandle = handle;
 
+		proxy->m_clientObject = 0;
+
 		m_numHandles--;
 	}