preparation for multi SAP broadphase

2007-08-03 01:39:36 +00:00
parent 1f7646f72a
commit b6cd61a5e1
9 changed files with 463 additions and 133 deletions
--- a/Demos/BasicDemo/BasicDemo.cpp
+++ b/Demos/BasicDemo/BasicDemo.cpp
@@ -25,6 +25,7 @@ subject to the following restrictions:
 int gNumObjects = 120;
 #define HALF_EXTENTS btScalar(1.)
 #include "btBulletDynamicsCommon.h"
+#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
 #include "LinearMath/btIDebugDraw.h"
 #include "GLDebugDrawer.h"
 #include <stdio.h> //printf debugging
@@ -56,6 +57,7 @@ class myTest

 int main(int argc,char** argv)
 {
+
 	BasicDemo ccdDemo;
 	ccdDemo.initPhysics();
 	ccdDemo.setCameraDistance(btScalar(50.));
@@ -157,7 +159,10 @@ void	BasicDemo::initPhysics()
 #define maxProxies 8192
 	btVector3 worldAabbMin(-10000,-10000,-10000);
 	btVector3 worldAabbMax(10000,10000,10000);
-	m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
+	//m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
+	m_overlappingPairCache = new btMultiSapBroadphase();
+
+
 #else
 	m_overlappingPairCache = new btSimpleBroadphase;
 #endif //USE_SWEEP_AND_PRUNE
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
@@ -45,10 +45,10 @@ void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality)
 #endif //DEBUG_BROADPHASE


-btBroadphaseProxy*	btAxisSweep3::createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+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(min,max, userPtr,collisionFilterGroup,collisionFilterMask);
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask);
 		
 		Handle* handle = getHandle(handleId);
 				
@@ -73,10 +73,16 @@ void	btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con



-btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles)
-:m_invalidPair(0)
+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());

@@ -140,6 +146,11 @@ 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
@@ -297,6 +308,8 @@ 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
@@ -369,6 +382,7 @@ void	btAxisSweep3::calculateOverlappingPairs()
 		m_invalidPair = 0;
 	#endif//CLEAN_INVALID_PAIRS
 		
+	}

 }

--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -92,6 +92,7 @@ protected:
 	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;

@@ -117,7 +118,7 @@ protected:
 	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);

 public:
-	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384);
+	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384, btOverlappingPairCache* pairCache=0);
 	virtual	~btAxisSweep3();

 	virtual void	calculateOverlappingPairs();
@@ -130,7 +131,7 @@ public:
 	void	processAllOverlappingPairs(btOverlapCallback* callback);

 	//Broadphase Interface
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	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);
 	
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
@@ -31,7 +31,7 @@ class btBroadphaseInterface
 public:
 	virtual ~btBroadphaseInterface() {}

-	virtual btBroadphaseProxy*	createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0;
+	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;
 	
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -79,8 +79,19 @@ struct btBroadphaseProxy

 	//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){}
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
@@ -0,0 +1,172 @@
+/*
+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"
+
+///	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);
+	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 = true;//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
+
+}
+
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
@@ -0,0 +1,111 @@
+/*
+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();
+
+	virtual	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_overlappingPairs;
+	}
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_overlappingPairs;
+	}
+};
+
+#endif //BT_MULTI_SAP_BROADPHASE
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -36,15 +36,21 @@ void	btSimpleBroadphase::validate()
 	
 }

-btSimpleBroadphase::btSimpleBroadphase(int maxProxies)
+btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
 	:
 	m_firstFreeProxy(0),
 	m_numProxies(0),
 	m_maxProxies(maxProxies),
-	m_invalidPair(0)
+	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];
@@ -71,20 +77,25 @@ btSimpleBroadphase::~btSimpleBroadphase()
 		destroyProxy(proxy);
 	}
 	*/
+
+	if (m_ownsPairCache)
+	{
+		delete m_pairCache;
+	}
 }


-btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask)
+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(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]);
+	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(min,max,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
+	btSimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
 	m_firstFreeProxy++;

 	btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
@@ -215,6 +226,8 @@ void	btSimpleBroadphase::calculateOverlappingPairs()
 		}
 	}

+	if (m_ownsPairCache)
+	{
 		
 		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();

@@ -287,6 +300,7 @@ void	btSimpleBroadphase::calculateOverlappingPairs()
 	#endif//CLEAN_INVALID_PAIRS
 	
 	}
+}


 bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
@@ -51,6 +51,7 @@ protected:
 	int				m_numProxies;

 	btOverlappingPairCache*	m_pairCache;
+	bool	m_ownsPairCache;

 	int	m_invalidPair;

@@ -69,18 +70,19 @@ protected:
 protected:


-	virtual void	calculateOverlappingPairs();
+	

 public:
-	btSimpleBroadphase(int maxProxies=16384);
+	btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0);
 	virtual ~btSimpleBroadphase();


 		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);


-	virtual btBroadphaseProxy*	createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	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);