initial commit

bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

@@ -0,0 +1,680 @@
+
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+
+//
+// btAxisSweep3
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#include "btAxisSweep3.h"
+
+#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& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+{
+		(void)shapeType;
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask);
+		
+		Handle* handle = getHandle(handleId);
+				
+		return handle;
+}
+
+void	btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	removeHandle(handle->m_handleId);
+}
+
+void	btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	updateHandle(handle->m_handleId,aabbMin,aabbMax);
+
+}
+
+
+
+
+
+
+btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles, btOverlappingPairCache* pairCache)
+:m_invalidPair(0),
+m_pairCache(pairCache),
+m_ownsPairCache(false)
+{
+	if (!m_pairCache)
+	{
+		m_pairCache = new btOverlappingPairCache();
+		m_ownsPairCache = true;
+	}
+
+	//assert(bounds.HasVolume());
+
+	// 1 handle is reserved as sentinel
+	btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES);
+
+	// init bounds
+	m_worldAabbMin = worldAabbMin;
+	m_worldAabbMax = worldAabbMax;
+
+	btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
+
+	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];
+	m_maxHandles = maxHandles;
+	m_numHandles = 0;
+
+	// handle 0 is reserved as the null index, and is also used as the sentinel
+	m_firstFreeHandle = 1;
+	{
+		for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
+			m_pHandles[i].SetNextFree(i + 1);
+		m_pHandles[maxHandles - 1].SetNextFree(0);
+	}
+
+	{
+	// allocate edge buffers
+	for (int i = 0; i < 3; i++)
+		m_pEdges[i] = new Edge[maxHandles * 2];
+	}
+	//removed overlap management
+
+	// make boundary sentinels
+	
+	m_pHandles[0].m_clientObject = 0;
+
+	for (int axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_minEdges[axis] = 0;
+		m_pHandles[0].m_maxEdges[axis] = 1;
+
+		m_pEdges[axis][0].m_pos = 0;
+		m_pEdges[axis][0].m_handle = 0;
+		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()
+{
+	
+	for (int i = 2; i >= 0; i--)
+		delete[] m_pEdges[i];
+	delete[] m_pHandles;
+
+	if (m_ownsPairCache)
+	{
+		delete m_pairCache;
+	}
+}
+
+void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
+{
+	btPoint3 clampedPoint(point);
+	
+
+
+	clampedPoint.setMax(m_worldAabbMin);
+	clampedPoint.setMin(m_worldAabbMax);
+
+	btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
+	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);
+	
+}
+
+
+
+BP_FP_INT_TYPE btAxisSweep3::allocHandle()
+{
+	assert(m_firstFreeHandle);
+
+	BP_FP_INT_TYPE handle = m_firstFreeHandle;
+	m_firstFreeHandle = getHandle(handle)->GetNextFree();
+	m_numHandles++;
+
+	return handle;
+}
+
+void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle)
+{
+	assert(handle > 0 && handle < m_maxHandles);
+
+	getHandle(handle)->SetNextFree(m_firstFreeHandle);
+	m_firstFreeHandle = handle;
+
+	m_numHandles--;
+}
+
+
+
+BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
+{
+	// quantize the bounds
+	BP_FP_INT_TYPE min[3], max[3];
+	quantize(min, aabbMin, 0);
+	quantize(max, aabbMax, 1);
+
+	// allocate a handle
+	BP_FP_INT_TYPE handle = allocHandle();
+	assert(handle!= 0xcdcd);
+
+	Handle* pHandle = getHandle(handle);
+	
+	pHandle->m_handleId = handle;
+	//pHandle->m_pOverlaps = 0;
+	pHandle->m_clientObject = pOwner;
+	pHandle->m_collisionFilterGroup = collisionFilterGroup;
+	pHandle->m_collisionFilterMask = collisionFilterMask;
+
+	// compute current limit of edge arrays
+	BP_FP_INT_TYPE limit = m_numHandles * 2;
+
+	
+	// insert new edges just inside the max boundary edge
+	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];
+
+		m_pEdges[axis][limit - 1].m_pos = min[axis];
+		m_pEdges[axis][limit - 1].m_handle = handle;
+
+		m_pEdges[axis][limit].m_pos = max[axis];
+		m_pEdges[axis][limit].m_handle = handle;
+
+		pHandle->m_minEdges[axis] = limit - 1;
+		pHandle->m_maxEdges[axis] = limit;
+	}
+
+	// now sort the new edges to their correct position
+	sortMinDown(0, pHandle->m_minEdges[0], false);
+	sortMaxDown(0, pHandle->m_maxEdges[0], false);
+	sortMinDown(1, pHandle->m_minEdges[1], false);
+	sortMaxDown(1, pHandle->m_maxEdges[1], false);
+	sortMinDown(2, pHandle->m_minEdges[2], true);
+	sortMaxDown(2, pHandle->m_maxEdges[2], true);
+
+
+	return handle;
+}
+
+
+void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
+{
+	
+	Handle* pHandle = getHandle(handle);
+
+	//explicitly remove the pairs containing the proxy
+	//we could do it also in the sortMinUp (passing true)
+	//todo: compare performance
+	m_pairCache->removeOverlappingPairsContainingProxy(pHandle);
+
+
+	// compute current limit of edge arrays
+	int limit = m_numHandles * 2;
+	
+	int axis;
+
+	for (axis = 0;axis<3;axis++)
+	{
+		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];
+		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
+		pEdges[max].m_pos = BP_HANDLE_SENTINEL;
+
+		sortMaxUp(axis,max,false);
+
+
+		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 = BP_HANDLE_SENTINEL;
+		
+#ifdef DEBUG_BROADPHASE
+			debugPrintAxis(axis,false);
+#endif //DEBUG_BROADPHASE
+
+
+	}
+
+
+	// free the handle
+	freeHandle(handle);
+
+	
+}
+
+extern int gOverlappingPairs;
+
+
+void	btAxisSweep3::calculateOverlappingPairs()
+{
+	
+	if (m_ownsPairCache)
+	{
+		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+
+		
+		int i;
+
+		btBroadphasePair previousPair;
+		previousPair.m_pProxy0 = 0;
+		previousPair.m_pProxy1 = 0;
+		previousPair.m_algorithm = 0;
+		
+		
+		for (i=0;i<overlappingPairArray.size();i++)
+		{
+		
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;//callback->processOverlap(pair);
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_pairCache->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--;
+			} 
+			
+		}
+
+	///if you don't like to skip the invalid pairs in the array, execute following code:
+	#define CLEAN_INVALID_PAIRS 1
+	#ifdef CLEAN_INVALID_PAIRS
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+	#endif//CLEAN_INVALID_PAIRS
+		
+	}
+
+}
+
+
+
+bool btAxisSweep3::testAabbOverlap(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
+
+	for (int axis = 0; axis < 3; axis++)
+	{ 
+		if (axis != ignoreAxis)
+		{
+			if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
+				pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
+			{ 
+				return false; 
+			} 
+		}
+	} 
+
+	//optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
+
+	/*for (int axis = 0; axis < 3; axis++)
+	{
+		if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
+			m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
+		{
+			return false;
+		}
+	}
+	*/
+
+	return true;
+}
+
+void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
+{
+//	assert(bounds.IsFinite());
+	//assert(bounds.HasVolume());
+
+	Handle* pHandle = getHandle(handle);
+
+	// quantize the new bounds
+	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++)
+	{
+		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;
+
+		m_pEdges[axis][emin].m_pos = min[axis];
+		m_pEdges[axis][emax].m_pos = max[axis];
+
+		// expand (only adds overlaps)
+		if (dmin < 0)
+			sortMinDown(axis, emin);
+
+		if (dmax > 0)
+			sortMaxUp(axis, emax);
+
+		// shrink (only removes overlaps)
+		if (dmin > 0)
+			sortMinUp(axis, emin);
+
+		if (dmax < 0)
+			sortMaxDown(axis, emax);
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+	}
+
+	
+}
+
+
+
+
+// sorting a min edge downwards can only ever *add* overlaps
+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);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+		if (pPrev->IsMax())
+		{
+			// if previous edge is a maximum check the bounds and add an overlap if necessary
+			if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
+			{
+				m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
+
+				//AddOverlap(pEdge->m_handle, pPrev->m_handle);
+
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_maxEdges[axis]++;
+		}
+		else
+			pHandlePrev->m_minEdges[axis]++;
+
+		pHandleEdge->m_minEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		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, BP_FP_INT_TYPE edge, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+	{
+		Handle* pHandleNext = getHandle(pNext->m_handle);
+
+		if (pNext->IsMax())
+		{
+			// 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);
+				*/
+
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_maxEdges[axis]--;
+		}
+		else
+			pHandleNext->m_minEdges[axis]--;
+
+		pHandleEdge->m_minEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+
+
+}
+
+// sorting a max edge downwards can only ever *remove* overlaps
+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);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+		if (!pPrev->IsMax())
+		{
+			// 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);
+				//assert(pair);
+
+				if (pair)
+				{
+					removeOverlappingPair(*pair);
+				}
+				*/
+
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_minEdges[axis]++;;
+		}
+		else
+			pHandlePrev->m_maxEdges[axis]++;
+
+		pHandleEdge->m_maxEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		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, BP_FP_INT_TYPE edge, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+	{
+		Handle* pHandleNext = getHandle(pNext->m_handle);
+
+		if (!pNext->IsMax())
+		{
+			// if next edge is a minimum check the bounds and add an overlap if necessary
+			if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
+			{
+				Handle* handle0 = getHandle(pEdge->m_handle);
+				Handle* handle1 = getHandle(pNext->m_handle);
+				m_pairCache->addOverlappingPair(handle0,handle1);
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_minEdges[axis]--;
+		}
+		else
+			pHandleNext->m_maxEdges[axis]--;
+
+		pHandleEdge->m_maxEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+	
+}
+
+

bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -0,0 +1,153 @@
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+//
+// btAxisSweep3.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifndef AXIS_SWEEP_3_H
+#define AXIS_SWEEP_3_H
+
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btVector3.h"
+#include "btOverlappingPairCache.h"
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+
+
+//Enable BP_USE_FIXEDPOINT_INT_32 if you need more then 32767 objects
+//#define BP_USE_FIXEDPOINT_INT_32 1
+
+#ifdef BP_USE_FIXEDPOINT_INT_32
+	#define BP_FP_INT_TYPE unsigned int
+	#define BP_MAX_HANDLES 1500000 //arbitrary maximum number of handles
+	#define BP_HANDLE_SENTINEL 0x7fffffff
+	#define BP_HANDLE_MASK	0xfffffffe
+#else
+	#define BP_FP_INT_TYPE unsigned short int
+	#define BP_MAX_HANDLES 32767
+	#define BP_HANDLE_SENTINEL 0xffff
+	#define BP_HANDLE_MASK	0xfffe
+#endif //BP_USE_FIXEDPOINT_INT_32
+
+//#define DEBUG_BROADPHASE 1
+
+/// btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
+/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats.
+/// The testOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos
+class btAxisSweep3 : public btBroadphaseInterface
+{
+
+public:
+	
+
+	class Edge
+	{
+	public:
+		BP_FP_INT_TYPE m_pos;			// low bit is min/max
+		BP_FP_INT_TYPE m_handle;
+
+		BP_FP_INT_TYPE IsMax() const {return m_pos & 1;}
+	};
+
+public:
+	class Handle : public btBroadphaseProxy
+	{
+	public:
+		
+		// indexes into the edge arrays
+		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
+		BP_FP_INT_TYPE m_handleId;
+		BP_FP_INT_TYPE m_pad;
+		
+		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
+	
+		inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
+		inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
+	};		// 24 bytes + 24 for Edge structures = 44 bytes total per entry
+
+	
+protected:
+	btPoint3 m_worldAabbMin;						// overall system bounds
+	btPoint3 m_worldAabbMax;						// overall system bounds
+
+	btVector3 m_quantize;						// scaling factor for quantization
+
+	BP_FP_INT_TYPE m_numHandles;						// number of active handles
+	int m_maxHandles;						// max number of handles
+	Handle* m_pHandles;						// handles pool
+	BP_FP_INT_TYPE m_firstFreeHandle;		// free handles list
+
+	Edge* m_pEdges[3];						// edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
+
+	btOverlappingPairCache* m_pairCache;
+	bool	m_ownsPairCache;
+
+	int	m_invalidPair;
+
+	// allocation/deallocation
+	BP_FP_INT_TYPE allocHandle();
+	void freeHandle(BP_FP_INT_TYPE handle);
+	
+
+	bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB);
+
+#ifdef DEBUG_BROADPHASE
+	void debugPrintAxis(int axis,bool checkCardinality=true);
+#endif //DEBUG_BROADPHASE
+
+	//Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+	//void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+
+	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
+
+	void sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
+	void sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
+	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
+	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
+
+public:
+	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384, btOverlappingPairCache* pairCache=0);
+	virtual	~btAxisSweep3();
+
+	virtual void	calculateOverlappingPairs();
+	
+	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
+	void removeHandle(BP_FP_INT_TYPE handle);
+	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax);
+	inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+
+	void	processAllOverlappingPairs(btOverlapCallback* callback);
+
+	//Broadphase Interface
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	virtual void	destroyProxy(btBroadphaseProxy* proxy);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_pairCache;
+	}
+	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_pairCache;
+	}
+
+
+};
+
+#endif
+

bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef		BROADPHASE_INTERFACE_H
+#define 	BROADPHASE_INTERFACE_H
+
+
+
+struct btDispatcherInfo;
+class btDispatcher;
+#include "btBroadphaseProxy.h"
+class btOverlappingPairCache;
+
+#include "LinearMath/btVector3.h"
+
+///BroadphaseInterface for aabb-overlapping object pairs
+class btBroadphaseInterface
+{
+public:
+	virtual ~btBroadphaseInterface() {}
+
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0;
+	virtual void	destroyProxy(btBroadphaseProxy* proxy)=0;
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0;
+	
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+	virtual void	calculateOverlappingPairs()=0;
+
+	virtual	btOverlappingPairCache*	getOverlappingPairCache()=0;
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const =0;
+
+};
+
+#endif //BROADPHASE_INTERFACE_H

bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp

@@ -0,0 +1,17 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBroadphaseProxy.h"
+

bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -0,0 +1,217 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BROADPHASE_PROXY_H
+#define BROADPHASE_PROXY_H
+
+#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+
+
+/// btDispatcher uses these types
+/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
+/// to facilitate type checking
+enum BroadphaseNativeTypes
+{
+// polyhedral convex shapes
+	BOX_SHAPE_PROXYTYPE,
+	TRIANGLE_SHAPE_PROXYTYPE,
+	TETRAHEDRAL_SHAPE_PROXYTYPE,
+	CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
+	CONVEX_HULL_SHAPE_PROXYTYPE,
+//implicit convex shapes
+IMPLICIT_CONVEX_SHAPES_START_HERE,
+	SPHERE_SHAPE_PROXYTYPE,
+	MULTI_SPHERE_SHAPE_PROXYTYPE,
+	CAPSULE_SHAPE_PROXYTYPE,
+	CONE_SHAPE_PROXYTYPE,
+	CONVEX_SHAPE_PROXYTYPE,
+	CYLINDER_SHAPE_PROXYTYPE,
+	UNIFORM_SCALING_SHAPE_PROXYTYPE,
+	MINKOWSKI_SUM_SHAPE_PROXYTYPE,
+	MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
+//concave shapes
+CONCAVE_SHAPES_START_HERE,
+	//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+	TRIANGLE_MESH_SHAPE_PROXYTYPE,
+	///used for demo integration FAST/Swift collision library and Bullet
+	FAST_CONCAVE_MESH_PROXYTYPE,
+	//terrain
+	TERRAIN_SHAPE_PROXYTYPE,
+///Used for GIMPACT Trimesh integration
+	GIMPACT_SHAPE_PROXYTYPE,
+	
+	EMPTY_SHAPE_PROXYTYPE,
+	STATIC_PLANE_PROXYTYPE,
+CONCAVE_SHAPES_END_HERE,
+
+	COMPOUND_SHAPE_PROXYTYPE,
+
+	MAX_BROADPHASE_COLLISION_TYPES
+};
+
+
+///btBroadphaseProxy
+struct btBroadphaseProxy
+{
+	
+	///optional filtering to cull potential collisions
+	enum CollisionFilterGroups
+	{
+	        DefaultFilter = 1,
+	        StaticFilter = 2,
+	        KinematicFilter = 4,
+	        DebrisFilter = 8,
+			SensorTrigger = 16,
+	        AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+	};
+
+	//Usually the client btCollisionObject or Rigidbody class
+	void*	m_clientObject;
+
+	///in the case of btMultiSapBroadphase, we store the collifionFilterGroup/Mask in the m_multiSapParentProxy
+	union
+	{
+		struct
+		{
+			short int m_collisionFilterGroup;
+			short int m_collisionFilterMask;
+		};
+
+		void*	m_multiSapParentProxy;
+
+	};
+
+	//used for memory pools
+	btBroadphaseProxy() :m_clientObject(0){}
+
+	btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask)
+		:m_clientObject(userPtr),
+		m_collisionFilterGroup(collisionFilterGroup),
+		m_collisionFilterMask(collisionFilterMask)
+	{
+	}
+
+	static inline bool isPolyhedral(int proxyType)
+	{
+		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
+	}
+
+	static inline bool	isConvex(int proxyType)
+	{
+		return (proxyType < CONCAVE_SHAPES_START_HERE);
+	}
+
+	static inline bool	isConcave(int proxyType)
+	{
+		return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
+			(proxyType < CONCAVE_SHAPES_END_HERE));
+	}
+	static inline bool	isCompound(int proxyType)
+	{
+		return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
+	}
+	static inline bool isInfinite(int proxyType)
+	{
+		return (proxyType == STATIC_PLANE_PROXYTYPE);
+	}
+	
+}
+;
+
+class btCollisionAlgorithm;
+
+struct btBroadphaseProxy;
+
+
+
+/// contains a pair of aabb-overlapping objects
+struct btBroadphasePair
+{
+	btBroadphasePair ()
+		:
+	m_pProxy0(0),
+		m_pProxy1(0),
+		m_algorithm(0),
+		m_userInfo(0)
+	{
+	}
+
+	btBroadphasePair(const btBroadphasePair& other)
+		:		m_pProxy0(other.m_pProxy0),
+				m_pProxy1(other.m_pProxy1),
+				m_algorithm(other.m_algorithm),
+				m_userInfo(other.m_userInfo)
+	{
+	}
+	btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+	{
+
+		//keep them sorted, so the std::set operations work
+		if (&proxy0 < &proxy1)
+        { 
+            m_pProxy0 = &proxy0; 
+            m_pProxy1 = &proxy1; 
+        }
+        else 
+        { 
+			m_pProxy0 = &proxy1; 
+            m_pProxy1 = &proxy0; 
+        }
+
+		m_algorithm = 0;
+		m_userInfo = 0;
+
+	}
+	
+	btBroadphaseProxy* m_pProxy0;
+	btBroadphaseProxy* m_pProxy1;
+	
+	mutable btCollisionAlgorithm* m_algorithm;
+	mutable void* m_userInfo;
+
+};
+
+/*
+//comparison for set operation, see Solid DT_Encounter
+SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) 
+{ 
+    return a.m_pProxy0 < b.m_pProxy0 || 
+        (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); 
+}
+*/
+
+
+
+class btBroadphasePairSortPredicate
+{
+	public:
+
+		bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
+		{
+			 return a.m_pProxy0 > b.m_pProxy0 || 
+				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
+				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); 
+		}
+};
+
+
+SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) 
+{
+	 return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
+}
+
+
+#endif //BROADPHASE_PROXY_H
+

bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp

@@ -0,0 +1,23 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btCollisionAlgorithm.h"
+#include "btDispatcher.h"
+
+btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+{
+	m_dispatcher = ci.m_dispatcher;
+}
+

bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h

@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_ALGORITHM_H
+#define COLLISION_ALGORITHM_H
+
+#include "LinearMath/btScalar.h"
+
+struct btBroadphaseProxy;
+class btDispatcher;
+class btManifoldResult;
+class btCollisionObject;
+struct btDispatcherInfo;
+class	btPersistentManifold;
+
+
+struct btCollisionAlgorithmConstructionInfo
+{
+	btCollisionAlgorithmConstructionInfo()
+		:m_dispatcher(0),
+		m_manifold(0)
+	{
+	}
+	btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
+		:m_dispatcher(dispatcher)
+	{
+		(void)temp;
+	}
+
+	btDispatcher*	m_dispatcher;
+	btPersistentManifold*	m_manifold;
+
+	int	getDispatcherId();
+
+};
+
+
+///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher.
+///It is persistent over frames
+class btCollisionAlgorithm
+{
+
+protected:
+
+	btDispatcher*	m_dispatcher;
+
+protected:
+	int	getDispatcherId();
+	
+public:
+
+	btCollisionAlgorithm() {};
+
+	btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+	virtual ~btCollisionAlgorithm() {};
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+
+};
+
+
+#endif //COLLISION_ALGORITHM_H

bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp

@@ -0,0 +1,22 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btDispatcher.h"
+
+btDispatcher::~btDispatcher()
+{
+
+}
+

bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -0,0 +1,93 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DISPATCHER_H
+#define _DISPATCHER_H
+
+#include "LinearMath/btScalar.h"
+
+class btCollisionAlgorithm;
+struct btBroadphaseProxy;
+class btRigidBody;
+class	btCollisionObject;
+class btOverlappingPairCache;
+
+
+class btPersistentManifold;
+class btStackAlloc;
+
+struct btDispatcherInfo
+{
+	enum DispatchFunc
+	{
+		DISPATCH_DISCRETE = 1,
+		DISPATCH_CONTINUOUS
+	};
+	btDispatcherInfo()
+		:m_timeStep(btScalar(0.)),
+		m_stepCount(0),
+		m_dispatchFunc(DISPATCH_DISCRETE),
+		m_timeOfImpact(btScalar(1.)),
+		m_useContinuous(false),
+		m_debugDraw(0),
+		m_enableSatConvex(false),
+		m_enableSPU(false),
+		m_stackAllocator(0)
+	{
+
+	}
+	btScalar	m_timeStep;
+	int		m_stepCount;
+	int		m_dispatchFunc;
+	btScalar	m_timeOfImpact;
+	bool	m_useContinuous;
+	class btIDebugDraw*	m_debugDraw;
+	bool	m_enableSatConvex;
+	bool	m_enableSPU;
+	btStackAlloc*	m_stackAllocator;
+	
+};
+
+/// btDispatcher can be used in combination with broadphase to dispatch overlapping pairs.
+/// For example for pairwise collision detection or user callbacks (game logic).
+class btDispatcher
+{
+
+
+public:
+	virtual ~btDispatcher() ;
+
+	virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold=0) = 0;
+
+	virtual btPersistentManifold*	getNewManifold(void* body0,void* body1)=0;
+
+	virtual void releaseManifold(btPersistentManifold* manifold)=0;
+
+	virtual void clearManifold(btPersistentManifold* manifold)=0;
+
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1) = 0;
+
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0;
+
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0;
+
+	virtual int getNumManifolds() const = 0;
+
+	virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;
+
+};
+
+
+#endif //_DISPATCHER_H

bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -0,0 +1,186 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMultiSapBroadphase.h"
+
+#include "btSimpleBroadphase.h"
+#include "LinearMath/btAabbUtil2.h"
+
+///	btSapBroadphaseArray	m_sapBroadphases;
+
+///	btOverlappingPairCache*	m_overlappingPairs;
+extern int gOverlappingPairs;
+
+btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies)
+:m_invalidPair(0)
+{
+	m_overlappingPairs = new btOverlappingPairCache();
+
+	struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
+	{
+		virtual ~btMultiSapOverlapFilterCallback()
+		{}
+		// return true when pairs need collision
+		virtual bool	needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
+		{
+			btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
+			btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
+			
+			bool collides = (multiSapProxy0->m_collisionFilterGroup & multiSapProxy1->m_collisionFilterMask) != 0;
+			collides = collides && (multiSapProxy1->m_collisionFilterGroup & multiSapProxy0->m_collisionFilterMask);
+	
+			return collides;
+		}
+	};
+
+	m_filterCallback = new btMultiSapOverlapFilterCallback();
+
+	m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
+
+	m_simpleBroadphase = new  btSimpleBroadphase(maxProxies,m_overlappingPairs);
+}
+
+btMultiSapBroadphase::~btMultiSapBroadphase()
+{
+}
+
+btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+{
+	btMultiSapProxy* proxy = new btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
+	m_multiSapProxies.push_back(proxy);
+
+	///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+	///this is needed to be able to calculate the aabb overlap
+	btBroadphaseProxy* simpleProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
+	simpleProxy->m_multiSapParentProxy = proxy;
+
+	btChildProxy* childProxyRef = new btChildProxy();
+	childProxyRef->m_proxy = simpleProxy;
+	childProxyRef->m_childBroadphase = m_simpleBroadphase;
+	proxy->m_childProxies.push_back(childProxyRef);
+
+	///this should deal with inserting/removal into child broadphases
+	setAabb(proxy,aabbMin,aabbMax);
+	return proxy;
+}
+
+void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy)
+{
+	///not yet
+	btAssert(0);
+
+}
+void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
+	multiProxy->m_aabbMin = aabbMin;
+	multiProxy->m_aabbMax = aabbMax;
+
+	for (int i=0;i<multiProxy->m_childProxies.size();i++)
+	{
+		btChildProxy* childProxyRef = multiProxy->m_childProxies[i];
+		childProxyRef->m_childBroadphase->setAabb(childProxyRef->m_proxy,aabbMin,aabbMax);
+	}
+
+}
+	
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+void	btMultiSapBroadphase::calculateOverlappingPairs()
+{
+	m_simpleBroadphase->calculateOverlappingPairs();
+
+	btBroadphasePairArray&	overlappingPairArray = m_overlappingPairs->getOverlappingPairArray();
+
+	//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+	overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+	overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+	m_invalidPair = 0;
+
+
+	btBroadphasePair previousPair;
+	previousPair.m_pProxy0 = 0;
+	previousPair.m_pProxy1 = 0;
+	previousPair.m_algorithm = 0;
+	
+	int i;
+
+	for (i=0;i<overlappingPairArray.size();i++)
+	{
+	
+		btBroadphasePair& pair = overlappingPairArray[i];
+
+		bool isDuplicate = (pair == previousPair);
+
+		previousPair = pair;
+
+		bool needsRemoval = false;
+
+		if (!isDuplicate)
+		{
+			bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+			if (hasOverlap)
+			{
+				needsRemoval = false;//callback->processOverlap(pair);
+			} else
+			{
+				needsRemoval = true;
+			}
+		} else
+		{
+			//remove duplicate
+			needsRemoval = true;
+			//should have no algorithm
+			btAssert(!pair.m_algorithm);
+		}
+		
+		if (needsRemoval)
+		{
+			m_overlappingPairs->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--;
+		} 
+		
+	}
+
+///if you don't like to skip the invalid pairs in the array, execute following code:
+#define CLEAN_INVALID_PAIRS 1
+#ifdef CLEAN_INVALID_PAIRS
+
+	//perform a sort, to sort 'invalid' pairs to the end
+	overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+	overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+	m_invalidPair = 0;
+#endif//CLEAN_INVALID_PAIRS
+
+}
+
+
+bool	btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
+{
+		btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
+		btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
+
+		return	TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
+			multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
+		
+}

bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -0,0 +1,113 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_MULTI_SAP_BROADPHASE
+#define BT_MULTI_SAP_BROADPHASE
+
+#include "btBroadphaseInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btOverlappingPairCache.h"
+
+class btAxisSweep3;
+class btSimpleBroadphase;
+
+
+typedef btAlignedObjectArray<btAxisSweep3*> btSapBroadphaseArray;
+
+///multi SAP broadphase
+///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
+///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
+class btMultiSapBroadphase :public btBroadphaseInterface
+{
+	btSapBroadphaseArray	m_sapBroadphases;
+	
+	btSimpleBroadphase*		m_simpleBroadphase;
+
+	btOverlappingPairCache*	m_overlappingPairs;
+	
+	btOverlapFilterCallback*	m_filterCallback;
+
+	int			m_invalidPair;
+
+	struct	btChildProxy
+	{
+		btBroadphaseProxy*		m_proxy;
+		btBroadphaseInterface*	m_childBroadphase;
+	};
+
+	struct	btMultiSapProxy	: public btBroadphaseProxy
+	{
+
+		///array with all the entries that this proxy belongs to
+		btAlignedObjectArray<btChildProxy*> m_childProxies;
+		btVector3	m_aabbMin;
+		btVector3	m_aabbMax;
+
+		int	m_shapeType;
+		void*	m_userPtr;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+
+		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+			:m_aabbMin(aabbMin),
+			m_aabbMax(aabbMax),
+			m_shapeType(shapeType),
+			m_userPtr(userPtr),
+			m_collisionFilterGroup(collisionFilterGroup),
+			m_collisionFilterMask(collisionFilterMask)
+		{
+
+		}
+
+		
+	};
+
+	btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
+
+public:
+
+	btMultiSapBroadphase(int maxProxies = 16384);
+
+	btSapBroadphaseArray	getBroadphaseArray()
+	{
+		return m_sapBroadphases;
+	}
+
+	const btSapBroadphaseArray	getBroadphaseArray() const
+	{
+		return m_sapBroadphases;
+	}
+
+	virtual ~btMultiSapBroadphase();
+
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask);
+	virtual void	destroyProxy(btBroadphaseProxy* proxy);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+	virtual void	calculateOverlappingPairs();
+
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	virtual	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_overlappingPairs;
+	}
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_overlappingPairs;
+	}
+};
+
+#endif //BT_MULTI_SAP_BROADPHASE

bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -0,0 +1,199 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btOverlappingPairCache.h"
+
+#include "btDispatcher.h"
+#include "btCollisionAlgorithm.h"
+
+int	gOverlappingPairs = 0;
+
+btOverlappingPairCache::btOverlappingPairCache():
+m_blockedForChanges(false),
+m_overlapFilterCallback(0)
+//m_NumOverlapBroadphasePair(0)
+{
+}
+
+
+btOverlappingPairCache::~btOverlappingPairCache()
+{
+	//todo/test: show we erase/delete data, or is it automatic
+}
+
+
+void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair)
+{
+	
+	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		gOverlappingPairs--;
+		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+		cleanOverlappingPair(pair);
+		
+		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+		m_overlappingPairArray.pop_back();
+	}
+}
+
+
+void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair)
+{
+	if (pair.m_algorithm)
+	{
+		{
+			delete pair.m_algorithm;;
+			pair.m_algorithm=0;
+		}
+	}
+}
+
+
+
+
+
+void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	//don't add overlap with own
+	assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return;
+
+
+	btBroadphasePair pair(*proxy0,*proxy1);
+	
+	m_overlappingPairArray.push_back(pair);
+	gOverlappingPairs++;
+	
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+ btBroadphasePair*	btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+
+	btBroadphasePair tmpPair(*proxy0,*proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//assert(it != m_overlappingPairSet.end());
+		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+
+
+
+
+void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy)
+{
+
+	class	CleanPairCallback : public btOverlapCallback
+	{
+		btBroadphaseProxy* m_cleanProxy;
+		btOverlappingPairCache*	m_pairCache;
+
+	public:
+		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache)
+			:m_cleanProxy(cleanProxy),
+			m_pairCache(pairCache)
+		{
+		}
+		virtual	bool	processOverlap(btBroadphasePair& pair)
+		{
+			if ((pair.m_pProxy0 == m_cleanProxy) ||
+				(pair.m_pProxy1 == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair);
+			}
+			return false;
+		}
+		
+	};
+
+	CleanPairCallback cleanPairs(proxy,this);
+
+	processAllOverlappingPairs(&cleanPairs);
+
+}
+
+
+
+void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy)
+{
+
+	class	RemovePairCallback : public btOverlapCallback
+	{
+		btBroadphaseProxy* m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
+			:m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual	bool	processOverlap(btBroadphasePair& pair)
+		{
+			return ((pair.m_pProxy0 == m_obsoleteProxy) ||
+				(pair.m_pProxy1 == m_obsoleteProxy));
+		}
+		
+	};
+
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback);
+}
+
+
+
+void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback)
+{
+
+	int i;
+
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		btBroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			cleanOverlappingPair(*pair);
+
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.pop_back();
+			gOverlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+
+
+

bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -0,0 +1,129 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OVERLAPPING_PAIR_CACHE_H
+#define OVERLAPPING_PAIR_CACHE_H
+
+
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+struct	btOverlapCallback
+{
+	virtual ~btOverlapCallback()
+	{}
+	//return true for deletion of the pair
+	virtual bool	processOverlap(btBroadphasePair& pair) = 0;
+};
+
+struct btOverlapFilterCallback
+{
+	virtual ~btOverlapFilterCallback()
+	{}
+	// return true when pairs need collision
+	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
+};
+
+typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
+
+///btOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
+class	btOverlappingPairCache
+{
+	protected:
+		//avoid brute-force finding all the time
+		btBroadphasePairArray	m_overlappingPairArray;
+		
+		//during the dispatch, check that user doesn't destroy/create proxy
+		bool		m_blockedForChanges;
+		
+		//if set, use the callback instead of the built in filter in needBroadphaseCollision
+		btOverlapFilterCallback* m_overlapFilterCallback;
+
+	public:
+			
+		btOverlappingPairCache();	
+		virtual ~btOverlappingPairCache();
+
+		virtual void	processAllOverlappingPairs(btOverlapCallback*);
+
+		void	removeOverlappingPair(btBroadphasePair& pair);
+
+		void	cleanOverlappingPair(btBroadphasePair& pair);
+		
+		void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+			
+		
+		void	cleanProxyFromPairs(btBroadphaseProxy* proxy);
+
+		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy);
+
+
+		inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+		{
+			if (m_overlapFilterCallback)
+				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			
+			return collides;
+		}
+		
+		btBroadphasePairArray&	getOverlappingPairArray()
+		{
+			return m_overlappingPairArray;
+		}
+
+		const btBroadphasePairArray&	getOverlappingPairArray() const
+		{
+			return m_overlappingPairArray;
+		}
+
+		btBroadphasePair*	getOverlappingPairArrayPtr()
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		const btBroadphasePair*	getOverlappingPairArrayPtr() const
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		int	getNumOverlappingPairs() const
+		{
+			return m_overlappingPairArray.size();
+		}
+		
+		btOverlapFilterCallback* getOverlapFilterCallback()
+		{
+			return m_overlapFilterCallback;
+		}
+
+		void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+		{
+			m_overlapFilterCallback = callback;
+		}
+
+};
+#endif //OVERLAPPING_PAIR_CACHE_H
+
+

bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -0,0 +1,314 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSimpleBroadphase.h"
+#include <BulletCollision/BroadphaseCollision/btDispatcher.h>
+#include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h>
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include <new>
+
+extern int gOverlappingPairs;
+
+void	btSimpleBroadphase::validate()
+{
+	for (int i=0;i<m_numProxies;i++)
+	{
+		for (int j=i+1;j<m_numProxies;j++)
+		{
+			assert(m_pProxies[i] != m_pProxies[j]);
+		}
+	}
+	
+}
+
+btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
+	:
+	m_firstFreeProxy(0),
+	m_numProxies(0),
+	m_maxProxies(maxProxies),
+	m_invalidPair(0),
+	m_pairCache(overlappingPairCache),
+	m_ownsPairCache(false)
+{
+
+	if (!overlappingPairCache)
+	{
+		m_pairCache = new btOverlappingPairCache();
+		m_ownsPairCache = true;
+	}
+
+	m_proxies = new btSimpleBroadphaseProxy[maxProxies];
+	m_freeProxies = new int[maxProxies];
+	m_pProxies = new btSimpleBroadphaseProxy*[maxProxies];
+	
+
+	int i;
+	for (i=0;i<m_maxProxies;i++)
+	{
+		m_freeProxies[i] = i;
+	}
+}
+
+btSimpleBroadphase::~btSimpleBroadphase()
+{
+	delete[] m_proxies;
+	delete []m_freeProxies;
+	delete [] m_pProxies;
+
+	/*int i;
+	for (i=m_numProxies-1;i>=0;i--)
+	{
+		BP_Proxy* proxy = m_pProxies[i]; 
+		destroyProxy(proxy);
+	}
+	*/
+
+	if (m_ownsPairCache)
+	{
+		delete m_pairCache;
+	}
+}
+
+
+btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask)
+{
+	if (m_numProxies >= m_maxProxies)
+	{
+		assert(0);
+		return 0; //should never happen, but don't let the game crash ;-)
+	}
+	assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
+
+	int freeIndex= m_freeProxies[m_firstFreeProxy];
+	btSimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
+	m_firstFreeProxy++;
+
+	btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
+		
+	int	index = int(proxy - proxy1);
+	btAssert(index == freeIndex);
+
+	m_pProxies[m_numProxies] = proxy;
+	m_numProxies++;
+	//validate();
+
+	return proxy;
+}
+
+class	RemovingOverlapCallback : public btOverlapCallback
+{
+protected:
+	virtual bool	processOverlap(btBroadphasePair& pair)
+	{
+		(void)pair;
+		btAssert(0);
+		return false;
+	}
+};
+
+class RemovePairContainingProxy
+{
+
+	btBroadphaseProxy*	m_targetProxy;
+	public:
+	virtual ~RemovePairContainingProxy()
+	{
+	}
+protected:
+	virtual bool processOverlap(btBroadphasePair& pair)
+	{
+		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
+		btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
+
+		return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
+	};
+};
+
+void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg)
+{
+		
+		int i;
+		
+		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
+		btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
+	
+		int index = int(proxy0 - proxy1);
+		btAssert (index < m_maxProxies);
+		m_freeProxies[--m_firstFreeProxy] = index;
+
+		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg);
+		
+		for (i=0;i<m_numProxies;i++)
+		{
+			if (m_pProxies[i] == proxyOrg)
+			{
+				m_pProxies[i] = m_pProxies[m_numProxies-1];
+				break;
+			}
+		}
+		m_numProxies--;
+		//validate();
+		
+}
+
+void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
+	sbp->m_min = aabbMin;
+	sbp->m_max = aabbMax;
+}
+
+
+
+
+
+	
+
+
+
+bool	btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
+{
+	return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && 
+		   proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] &&
+		   proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2];
+
+}
+
+
+
+//then remove non-overlapping ones
+class CheckOverlapCallback : public btOverlapCallback
+{
+public:
+	virtual bool processOverlap(btBroadphasePair& pair)
+	{
+		return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
+	}
+};
+
+void	btSimpleBroadphase::calculateOverlappingPairs()
+{
+	//first check for new overlapping pairs
+	int i,j;
+
+	for (i=0;i<m_numProxies;i++)
+	{
+		btBroadphaseProxy* proxy0 = m_pProxies[i];
+		for (j=i+1;j<m_numProxies;j++)
+		{
+			btBroadphaseProxy* proxy1 = m_pProxies[j];
+			btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+			btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+
+			if (aabbOverlap(p0,p1))
+			{
+				if ( !m_pairCache->findPair(proxy0,proxy1))
+				{
+					m_pairCache->addOverlappingPair(proxy0,proxy1);
+				}
+			}
+
+		}
+	}
+
+	if (m_ownsPairCache)
+	{
+		
+		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+
+
+		btBroadphasePair previousPair;
+		previousPair.m_pProxy0 = 0;
+		previousPair.m_pProxy1 = 0;
+		previousPair.m_algorithm = 0;
+		
+		
+		for (i=0;i<overlappingPairArray.size();i++)
+		{
+		
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;//callback->processOverlap(pair);
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_pairCache->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--;
+			} 
+			
+		}
+
+	///if you don't like to skip the invalid pairs in the array, execute following code:
+	#define CLEAN_INVALID_PAIRS 1
+	#ifdef CLEAN_INVALID_PAIRS
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+	#endif//CLEAN_INVALID_PAIRS
+	
+	}
+}
+
+
+bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+	btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+	return aabbOverlap(p0,p1);
+}
+
+
+

bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SIMPLE_BROADPHASE_H
+#define SIMPLE_BROADPHASE_H
+
+
+#include "btOverlappingPairCache.h"
+
+
+struct btSimpleBroadphaseProxy : public btBroadphaseProxy
+{
+	btVector3	m_min;
+	btVector3	m_max;
+	
+	btSimpleBroadphaseProxy() {};
+
+	btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+	:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
+	m_min(minpt),m_max(maxpt)		
+	{
+		(void)shapeType;
+	}
+	
+
+};
+
+///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks
+class btSimpleBroadphase : public btBroadphaseInterface
+{
+
+protected:
+
+	btSimpleBroadphaseProxy*	m_proxies;
+	int*				m_freeProxies;
+	int				m_firstFreeProxy;
+
+	btSimpleBroadphaseProxy** m_pProxies;
+	int				m_numProxies;
+
+	btOverlappingPairCache*	m_pairCache;
+	bool	m_ownsPairCache;
+
+	int	m_invalidPair;
+
+	int m_maxProxies;
+	
+	
+	inline btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
+	{
+		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
+
+	void	validate();
+
+protected:
+
+
+	
+
+public:
+	btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0);
+	virtual ~btSimpleBroadphase();
+
+
+		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
+
+
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+
+	virtual void	calculateOverlappingPairs();
+
+	virtual void	destroyProxy(btBroadphaseProxy* proxy);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+		
+	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_pairCache;
+	}
+	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_pairCache;
+	}
+
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+
+};
+
+
+
+#endif //SIMPLE_BROADPHASE_H
+