1 files changed, 211 insertions, 50 deletions

diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
index b05285ca727..be4a11506df 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
@@ -21,9 +21,34 @@
 
 #include <assert.h>
 
+#ifdef DEBUG_BROADPHASE
+#include <stdio.h>
+void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality)
+{
+	int numEdges = m_pHandles[0].m_maxEdges[axis];
+	printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
+
+	int i;
+	for (i=0;i<numEdges+1;i++)
+	{
+		Edge* pEdge = m_pEdges[axis] + i;
+		Handle* pHandlePrev = getHandle(pEdge->m_handle);
+		int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
+		char beginOrEnd;
+		beginOrEnd=pEdge->IsMax()?'E':'B';
+		printf("	[%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
+	}
+
+	if (checkCardinality)
+		assert(numEdges == m_numHandles*2+1);
+}
+#endif //DEBUG_BROADPHASE
+
+
 btBroadphaseProxy*	btAxisSweep3::createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
 {
-		unsigned short handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask);
+		(void)shapeType;
+		BP_FP_INT_TYPE handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask);
 		
 		Handle* handle = getHandle(handleId);
 				
@@ -40,6 +65,7 @@ void	btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con
 {
 	Handle* handle = static_cast<Handle*>(proxy);
 	updateHandle(handle->m_handleId,aabbMin,aabbMax);
+
 }
 
 
@@ -50,10 +76,11 @@ void	btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con
 btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles)
 :btOverlappingPairCache()
 {
+	m_invalidPair = 0;
 	//assert(bounds.HasVolume());
 
 	// 1 handle is reserved as sentinel
-	assert(maxHandles > 1 && maxHandles < 32767);
+	btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES);
 
 	// init bounds
 	m_worldAabbMin = worldAabbMin;
@@ -61,7 +88,9 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab
 
 	btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
 
-	m_quantize = btVector3(65535.0f,65535.0f,65535.0f) / aabbSize;
+	BP_FP_INT_TYPE	maxInt = BP_HANDLE_SENTINEL;
+
+	m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
 
 	// allocate handles buffer and put all handles on free list
 	m_pHandles = new Handle[maxHandles];
@@ -71,7 +100,7 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab
 	// handle 0 is reserved as the null index, and is also used as the sentinel
 	m_firstFreeHandle = 1;
 	{
-		for (int i = m_firstFreeHandle; i < maxHandles; i++)
+		for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
 			m_pHandles[i].SetNextFree(i + 1);
 		m_pHandles[maxHandles - 1].SetNextFree(0);
 	}
@@ -94,9 +123,14 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab
 
 		m_pEdges[axis][0].m_pos = 0;
 		m_pEdges[axis][0].m_handle = 0;
-		m_pEdges[axis][1].m_pos = 0xffff;
+		m_pEdges[axis][1].m_pos = BP_HANDLE_SENTINEL;
 		m_pEdges[axis][1].m_handle = 0;
+#ifdef DEBUG_BROADPHASE
+		debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
 	}
+
 }
 
 btAxisSweep3::~btAxisSweep3()
@@ -107,43 +141,36 @@ btAxisSweep3::~btAxisSweep3()
 	delete[] m_pHandles;
 }
 
-void btAxisSweep3::quantize(unsigned short* out, const btPoint3& point, int isMax) const
+void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
 {
 	btPoint3 clampedPoint(point);
-	/*
-	if (isMax)
-		clampedPoint += btVector3(10,10,10);
-	else
-	{
-		clampedPoint -= btVector3(10,10,10);
-	}
-	*/
+	
 
 
 	clampedPoint.setMax(m_worldAabbMin);
 	clampedPoint.setMin(m_worldAabbMax);
 
 	btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
-	out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax);
-	out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax);
-	out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax);
+	out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & BP_HANDLE_MASK) | isMax);
+	out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & BP_HANDLE_MASK) | isMax);
+	out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & BP_HANDLE_MASK) | isMax);
 	
 }
 
 
 
-unsigned short btAxisSweep3::allocHandle()
+BP_FP_INT_TYPE btAxisSweep3::allocHandle()
 {
 	assert(m_firstFreeHandle);
 
-	unsigned short handle = m_firstFreeHandle;
+	BP_FP_INT_TYPE handle = m_firstFreeHandle;
 	m_firstFreeHandle = getHandle(handle)->GetNextFree();
 	m_numHandles++;
 
 	return handle;
 }
 
-void btAxisSweep3::freeHandle(unsigned short handle)
+void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle)
 {
 	assert(handle > 0 && handle < m_maxHandles);
 
@@ -155,15 +182,15 @@ void btAxisSweep3::freeHandle(unsigned short handle)
 
 
 
-unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
+BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
 {
 	// quantize the bounds
-	unsigned short min[3], max[3];
+	BP_FP_INT_TYPE min[3], max[3];
 	quantize(min, aabbMin, 0);
 	quantize(max, aabbMax, 1);
 
 	// allocate a handle
-	unsigned short handle = allocHandle();
+	BP_FP_INT_TYPE handle = allocHandle();
 	assert(handle!= 0xcdcd);
 
 	Handle* pHandle = getHandle(handle);
@@ -175,11 +202,13 @@ unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a
 	pHandle->m_collisionFilterMask = collisionFilterMask;
 
 	// compute current limit of edge arrays
-	int limit = m_numHandles * 2;
+	BP_FP_INT_TYPE limit = m_numHandles * 2;
 
+	
 	// insert new edges just inside the max boundary edge
-	for (int axis = 0; axis < 3; axis++)
+	for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
 	{
+
 		m_pHandles[0].m_maxEdges[axis] += 2;
 
 		m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
@@ -202,14 +231,14 @@ unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a
 	sortMinDown(2, pHandle->m_minEdges[2], true);
 	sortMaxDown(2, pHandle->m_maxEdges[2], true);
 
-	//PrintAxis(1);
 
 	return handle;
 }
 
 
-void btAxisSweep3::removeHandle(unsigned short handle)
+void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
 {
+	
 	Handle* pHandle = getHandle(handle);
 
 	//explicitly remove the pairs containing the proxy
@@ -220,42 +249,145 @@ void btAxisSweep3::removeHandle(unsigned short handle)
 
 	// compute current limit of edge arrays
 	int limit = m_numHandles * 2;
+	
 	int axis;
 
 	for (axis = 0;axis<3;axis++)
 	{
-		Edge* pEdges = m_pEdges[axis];
-		int maxEdge= pHandle->m_maxEdges[axis];
-		pEdges[maxEdge].m_pos = 0xffff;
-		int minEdge = pHandle->m_minEdges[axis];
-		pEdges[minEdge].m_pos = 0xffff;
+		m_pHandles[0].m_maxEdges[axis] -= 2;
 	}
 
 	// remove the edges by sorting them up to the end of the list
 	for ( axis = 0; axis < 3; axis++)
 	{
 		Edge* pEdges = m_pEdges[axis];
-		int max = pHandle->m_maxEdges[axis];
-		pEdges[max].m_pos = 0xffff;
+		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
+		pEdges[max].m_pos = BP_HANDLE_SENTINEL;
 
 		sortMaxUp(axis,max,false);
-		
-		int i = pHandle->m_minEdges[axis];
-		pEdges[i].m_pos = 0xffff;
+
+
+		BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
+		pEdges[i].m_pos = BP_HANDLE_SENTINEL;
+
 
 		sortMinUp(axis,i,false);
 
 		pEdges[limit-1].m_handle = 0;
-		pEdges[limit-1].m_pos = 0xffff;
+		pEdges[limit-1].m_pos = BP_HANDLE_SENTINEL;
+		
+#ifdef DEBUG_BROADPHASE
+			debugPrintAxis(axis,false);
+#endif //DEBUG_BROADPHASE
+
 
 	}
 
+
 	// free the handle
 	freeHandle(handle);
 
 	
 }
 
+extern int gOverlappingPairs;
+
+
+void	btAxisSweep3::refreshOverlappingPairs()
+{
+
+}
+void	btAxisSweep3::processAllOverlappingPairs(btOverlapCallback* callback)
+{
+
+	//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+	m_overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+	//remove the 'invalid' ones
+#ifdef USE_POPBACK_REMOVAL
+	while (m_invalidPair>0)
+	{
+		m_invalidPair--;
+		m_overlappingPairArray.pop_back();
+	}
+#else	
+	m_overlappingPairArray.resize(m_overlappingPairArray.size() - m_invalidPair);
+	m_invalidPair = 0;
+#endif
+
+	
+	int i;
+
+	btBroadphasePair previousPair;
+	previousPair.m_pProxy0 = 0;
+	previousPair.m_pProxy1 = 0;
+	previousPair.m_algorithm = 0;
+	
+	
+	for (i=0;i<m_overlappingPairArray.size();i++)
+	{
+	
+		btBroadphasePair& pair = m_overlappingPairArray[i];
+
+		bool isDuplicate = (pair == previousPair);
+
+		previousPair = pair;
+
+		bool needsRemoval = false;
+
+		if (!isDuplicate)
+		{
+			bool hasOverlap = testOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+			if (hasOverlap)
+			{
+				needsRemoval = callback->processOverlap(pair);
+			} else
+			{
+				needsRemoval = true;
+			}
+		} else
+		{
+			//remove duplicate
+			needsRemoval = true;
+			//should have no algorithm
+			btAssert(!pair.m_algorithm);
+		}
+		
+		if (needsRemoval)
+		{
+			cleanOverlappingPair(pair);
+
+	//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+	//		m_overlappingPairArray.pop_back();
+			pair.m_pProxy0 = 0;
+			pair.m_pProxy1 = 0;
+			m_invalidPair++;
+			gOverlappingPairs--;
+		} 
+		
+	}
+}
+
+
+bool btAxisSweep3::testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	const Handle* pHandleA = static_cast<Handle*>(proxy0);
+	const Handle* pHandleB = static_cast<Handle*>(proxy1);
+	
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	for (int axis = 0; axis < 3; axis++)
+	{ 
+		if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
+			pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
+		{ 
+			return false; 
+		} 
+	} 
+	return true;
+}
+
 bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
 {
 	//optimization 1: check the array index (memory address), instead of the m_pos
@@ -272,7 +404,7 @@ bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Hand
 		}
 	} 
 
-	//optimization 2: only 2 axis need to be tested
+	//optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
 
 	/*for (int axis = 0; axis < 3; axis++)
 	{
@@ -287,7 +419,7 @@ bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Hand
 	return true;
 }
 
-void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
+void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
 {
 //	assert(bounds.IsFinite());
 	//assert(bounds.HasVolume());
@@ -295,15 +427,15 @@ void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,c
 	Handle* pHandle = getHandle(handle);
 
 	// quantize the new bounds
-	unsigned short min[3], max[3];
+	BP_FP_INT_TYPE min[3], max[3];
 	quantize(min, aabbMin, 0);
 	quantize(max, aabbMax, 1);
 
 	// update changed edges
 	for (int axis = 0; axis < 3; axis++)
 	{
-		unsigned short emin = pHandle->m_minEdges[axis];
-		unsigned short emax = pHandle->m_maxEdges[axis];
+		BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
+		BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
 
 		int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
 		int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
@@ -324,14 +456,22 @@ void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,c
 
 		if (dmax < 0)
 			sortMaxDown(axis, emax);
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
 	}
 
-	//PrintAxis(1);
+	
 }
 
+
+
+
 // sorting a min edge downwards can only ever *add* overlaps
-void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlaps)
+void btAxisSweep3::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
 {
+
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pPrev = pEdge - 1;
 	Handle* pHandleEdge = getHandle(pEdge->m_handle);
@@ -368,16 +508,21 @@ void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlap
 		pEdge--;
 		pPrev--;
 	}
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
 }
 
 // sorting a min edge upwards can only ever *remove* overlaps
-void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps)
+void btAxisSweep3::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
 {
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pNext = pEdge + 1;
 	Handle* pHandleEdge = getHandle(pEdge->m_handle);
 
-	while (pEdge->m_pos > pNext->m_pos)
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
 	{
 		Handle* pHandleNext = getHandle(pNext->m_handle);
 
@@ -386,10 +531,12 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps)
 			// if next edge is maximum remove any overlap between the two handles
 			if (updateOverlaps)
 			{
+				/*
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pNext->m_handle);
 				btBroadphasePair tmpPair(*handle0,*handle1);
 				removeOverlappingPair(tmpPair);
+				*/
 
 			}
 
@@ -410,11 +557,14 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps)
 		pEdge++;
 		pNext++;
 	}
+
+
 }
 
 // sorting a max edge downwards can only ever *remove* overlaps
-void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlaps)
+void btAxisSweep3::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
 {
+
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pPrev = pEdge - 1;
 	Handle* pHandleEdge = getHandle(pEdge->m_handle);
@@ -428,6 +578,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap
 			// if previous edge was a minimum remove any overlap between the two handles
 			if (updateOverlaps)
 			{
+				//this is done during the overlappingpairarray iteration/narrowphase collision
+				/*
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pPrev->m_handle);
 				btBroadphasePair* pair = findPair(handle0,handle1);
@@ -437,6 +589,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap
 				{
 					removeOverlappingPair(*pair);
 				}
+				*/
+
 			}
 
 			// update edge reference in other handle
@@ -456,16 +610,22 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap
 		pEdge--;
 		pPrev--;
 	}
+
+	
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
 }
 
 // sorting a max edge upwards can only ever *add* overlaps
-void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps)
+void btAxisSweep3::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
 {
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pNext = pEdge + 1;
 	Handle* pHandleEdge = getHandle(pEdge->m_handle);
 
-	while (pEdge->m_pos > pNext->m_pos)
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
 	{
 		Handle* pHandleNext = getHandle(pNext->m_handle);
 
@@ -496,4 +656,5 @@ void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps)
 		pEdge++;
 		pNext++;
 	}
+	
 }