Improved speed for 'slow' moving bodies.

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -20,17 +20,18 @@ subject to the following restrictions:
 // Profiling
 //
 
-#if DBVT_BP_PROFILE
+#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
 #include <stdio.h>
+#endif
+
+#if DBVT_BP_PROFILE
 struct	ProfileScope
 	{
-	ProfileScope(btClock& clock,unsigned long& value)
+	__forceinline ProfileScope(btClock& clock,unsigned long& value) :
+		m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
 		{
-		m_clock=&clock;
-		m_value=&value;
-		m_base=clock.getTimeMicroseconds();
 		}
-	~ProfileScope()
+	__forceinline ~ProfileScope()
 		{
 		(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
 		}
@@ -93,14 +94,15 @@ btDbvtBroadphase*	pbp;
 		btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
 void	Process(const btDbvtNode* na,const btDbvtNode* nb)
 	{
-	btDbvtProxy*	pa=(btDbvtProxy*)na->data;
+	if(na!=nb)
-	btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
-	#if DBVT_BP_DISCRETPAIRS
-	if(Intersect(pa->aabb,pb->aabb))
-	#endif
 		{
+		btDbvtProxy*	pa=(btDbvtProxy*)na->data;
+		btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
+		#if DBVT_BP_SORTPAIRS
 		if(pa>pb) btSwap(pa,pb);
+		#endif
 		pbp->m_paircache->addOverlappingPair(pa,pb);
+		++pbp->m_newpairs;
 		}
 	}
 };
@@ -112,16 +114,23 @@ void	Process(const btDbvtNode* na,const btDbvtNode* nb)
 //
 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 {
+m_deferedcollide	=	false;
 m_releasepaircache	=	(paircache!=0)?false:true;
-m_predictedframes	=	2;
+m_predictedframes	=	15;
 m_stageCurrent		=	0;
 m_fupdates			=	1;
 m_dupdates			=	1;
+m_cupdates			=	10;
+m_newpairs			=	1;
+m_updates_call		=	0;
+m_updates_done		=	0;
+m_updates_ratio		=	0;
 m_paircache			=	paircache?
-							paircache	:
+						paircache	:
-							new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+						new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
 m_gid				=	0;
 m_pid				=	0;
+m_cid				=	0;
 for(int i=0;i<=STAGECOUNT;++i)
 	{
 	m_stageRoots[i]=0;
@@ -148,17 +157,18 @@ btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
 																void* userPtr,
 																short int collisionFilterGroup,
 																short int collisionFilterMask,
-																btDispatcher* /*dispatcher*/,
+																btDispatcher* dispatcher,
 																void* /*multiSapProxy*/)
 {
 btDbvtProxy*	proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	userPtr,
 																				collisionFilterGroup,
 																				collisionFilterMask);
-proxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
+proxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMin);
 proxy->leaf			=	m_sets[0].insert(proxy->aabb,proxy);
 proxy->stage		=	m_stageCurrent;
 proxy->m_uniqueId	=	++m_gid;
 listappend(proxy,m_stageRoots[m_stageCurrent]);
+setAabb(proxy,aabbMin,aabbMax,dispatcher);
 return(proxy);
 }
 
@@ -184,33 +194,50 @@ void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 {
 btDbvtProxy*	proxy=(btDbvtProxy*)absproxy;
 btDbvtVolume	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
-if(NotEqual(aabb,proxy->leaf->volume))
+//if(NotEqual(aabb,proxy->leaf->volume))
 	{
+	bool	docollide=false;
 	if(proxy->stage==STAGECOUNT)
 		{/* fixed -> dynamic set	*/ 
 		m_sets[1].remove(proxy->leaf);
 		proxy->leaf=m_sets[0].insert(aabb,proxy);
+		docollide=true;
 		}
 		else
 		{/* dynamic set				*/ 
+		++m_updates_call;
 		if(Intersect(proxy->leaf->volume,aabb))
 			{/* Moving				*/ 
 			const btVector3	delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
-			#ifdef DBVT_BP_MARGIN
+			if	(
-			m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes,DBVT_BP_MARGIN);
+				#ifdef DBVT_BP_MARGIN
-			#else
+				m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes,DBVT_BP_MARGIN)
-			m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes);
+				#else
-			#endif
+				m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes)
+				#endif
+				)
+				{
+				++m_updates_done;
+				docollide=true;
+				}
 			}
 			else
 			{/* Teleporting			*/ 
-			m_sets[0].update(proxy->leaf,aabb);		
+			m_sets[0].update(proxy->leaf,aabb);
+			++m_updates_done;
+			docollide=true;
 			}	
 		}
 	listremove(proxy,m_stageRoots[proxy->stage]);
 	proxy->aabb		=	aabb;
 	proxy->stage	=	m_stageCurrent;
 	listappend(proxy,m_stageRoots[m_stageCurrent]);
+	if(docollide&&(!m_deferedcollide))
+		{
+		btDbvtTreeCollider	collider(this);
+		btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
+		btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
+		}
 	}
 }
 
@@ -266,10 +293,12 @@ if(current)
 /* collide dynamics		*/ 
 	{
 	btDbvtTreeCollider	collider(this);
+	if(m_deferedcollide)
 		{
 		SPC(m_profiling.m_fdcollide);
 		btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
 		}
+	if(m_deferedcollide)
 		{
 		SPC(m_profiling.m_ddcollide);
 		btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
@@ -281,21 +310,36 @@ if(current)
 	btBroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
 	if(pairs.size()>0)
 		{
-		for(int i=0,ni=pairs.size();i<ni;++i)
+		const int	ci=pairs.size();
+		int			ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
+		for(int i=0;i<ni;++i)
 			{
-			btBroadphasePair&	p=pairs[i];
+			btBroadphasePair&	p=pairs[(m_cid+i)%ci];
-			btDbvtProxy*	pa=(btDbvtProxy*)p.m_pProxy0;
+			btDbvtProxy*		pa=(btDbvtProxy*)p.m_pProxy0;
-			btDbvtProxy*	pb=(btDbvtProxy*)p.m_pProxy1;
+			btDbvtProxy*		pb=(btDbvtProxy*)p.m_pProxy1;
-			if(!Intersect(pa->aabb,pb->aabb))
+			if((pa->stage==0)||(pb->stage==0))
 				{
-				if(pa>pb) btSwap(pa,pb);
+				if(!Intersect(pa->leaf->volume,pb->leaf->volume))
-				m_paircache->removeOverlappingPair(pa,pb,dispatcher);
+					{
-				--ni;--i;
+					#if DBVT_BP_SORTPAIRS
+					if(pa>pb) btSwap(pa,pb);
+					#endif
+					m_paircache->removeOverlappingPair(pa,pb,dispatcher);
+					--ni;--i;
+					}
 				}
 			}
+		if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
 		}
 	}
 ++m_pid;
+m_newpairs=1;
+if(m_updates_call>0)
+	{ m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
+	else
+	{ m_updates_ratio=0; }
+m_updates_done/=2;
+m_updates_call/=2;
 }
 
 //
@@ -339,6 +383,131 @@ aabbMax=bounds.Maxs();
 void							btDbvtBroadphase::printStats()
 {}
 
+//
+#if DBVT_BP_ENABLE_BENCHMARK
+
+struct	btBroadphaseBenchmark
+	{
+	struct	Experiment
+		{
+		const char*			name;
+		int					object_count;
+		int					update_count;
+		int					spawn_count;
+		int					iterations;
+		btScalar			speed;
+		btScalar			amplitude;
+		};
+	struct	Object
+		{
+		btVector3			center;
+		btVector3			extents;
+		btBroadphaseProxy*	proxy;
+		btScalar			time;
+		void				update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
+			{
+			time		+=	speed;
+			center[0]	=	btCos(time*(btScalar)2.17)*amplitude+
+							btSin(time)*amplitude/2;
+			center[1]	=	btCos(time*(btScalar)1.38)*amplitude+
+							btSin(time)*amplitude;
+			center[2]	=	btSin(time*(btScalar)0.777)*amplitude;
+			pbi->setAabb(proxy,center-extents,center+extents,0);
+			}
+		};
+	static int		UnsignedRand(int range=RAND_MAX-1)	{ return(rand()%(range+1)); }
+	static btScalar	UnitRand()							{ return(UnsignedRand(16384)/(btScalar)16384); }
+	static void		OutputTime(const char* name,btClock& c,unsigned count=0)
+		{
+		const unsigned long	us=c.getTimeMicroseconds();
+		const unsigned long	ms=(us+500)/1000;
+		const btScalar		sec=us/(btScalar)(1000*1000);
+		if(count>0)
+			printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
+			else
+			printf("%s : %u us (%u ms)\r\n",name,us,ms);
+		}
+	};
+
+void							btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
+{
+static const btBroadphaseBenchmark::Experiment		experiments[]=
+	{
+	{"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
+	/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
+	{"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
+	};
+static const int										nexperiments=sizeof(experiments)/sizeof(experiments[0]);
+btAlignedObjectArray<btBroadphaseBenchmark::Object*>	objects;
+btClock													wallclock;
+/* Begin			*/ 
+for(int iexp=0;iexp<nexperiments;++iexp)
+	{
+	const btBroadphaseBenchmark::Experiment&	experiment=experiments[iexp];
+	const int									object_count=experiment.object_count;
+	const int									update_count=(object_count*experiment.update_count)/100;
+	const int									spawn_count=(object_count*experiment.spawn_count)/100;
+	const btScalar								speed=experiment.speed;	
+	const btScalar								amplitude=experiment.amplitude;
+	printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
+	printf("\tObjects: %u\r\n",object_count);
+	printf("\tUpdate: %u\r\n",update_count);
+	printf("\tSpawn: %u\r\n",spawn_count);
+	printf("\tSpeed: %f\r\n",speed);
+	printf("\tAmplitude: %f\r\n",amplitude);
+	srand(180673);
+	/* Create objects	*/ 
+	wallclock.reset();
+	objects.reserve(object_count);
+	for(int i=0;i<object_count;++i)
+		{
+		btBroadphaseBenchmark::Object*	po=new btBroadphaseBenchmark::Object();
+		po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
+		po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
+		po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
+		po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
+		po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
+		po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
+		po->time=btBroadphaseBenchmark::UnitRand()*2000;
+		po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
+		objects.push_back(po);
+		}
+	btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
+	/* First update		*/ 
+	wallclock.reset();
+	for(int i=0;i<objects.size();++i)
+		{
+		objects[i]->update(speed,amplitude,pbi);
+		}
+	btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
+	/* Updates			*/ 
+	wallclock.reset();
+	for(int i=0;i<experiment.iterations;++i)
+		{
+		for(int j=0;j<update_count;++j)
+			{				
+			objects[j]->update(speed,amplitude,pbi);
+			}
+		pbi->calculateOverlappingPairs(0);
+		}
+	btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
+	/* Clean up			*/ 
+	wallclock.reset();
+	for(int i=0;i<objects.size();++i)
+		{
+		pbi->destroyProxy(objects[i]->proxy,0);
+		delete objects[i];
+		}
+	objects.resize(0);
+	btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
+	}
+
+}
+#else
+void							btDbvtBroadphase::benchmark(btBroadphaseInterface*)
+{}
+#endif
+
 #if DBVT_BP_PROFILE
 #undef	SPC
 #endif

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -23,9 +23,10 @@ subject to the following restrictions:
 // Compile time config
 //
 
-#define	DBVT_BP_PROFILE			0
+#define	DBVT_BP_PROFILE				0
-#define DBVT_BP_DISCRETPAIRS	1
+#define DBVT_BP_SORTPAIRS			1
-#define DBVT_BP_MARGIN			(btScalar)0.05
+#define DBVT_BP_ENABLE_BENCHMARK	0
+#define DBVT_BP_MARGIN				(btScalar)0.05
 
 #if DBVT_BP_PROFILE
 	#define	DBVT_BP_PROFILING_RATE	256
@@ -38,10 +39,10 @@ subject to the following restrictions:
 struct btDbvtProxy : btBroadphaseProxy
 {
 /* Fields		*/ 
-btDbvtAabbMm		aabb;
+btDbvtAabbMm	aabb;
 btDbvtNode*		leaf;
-btDbvtProxy*		links[2];
+btDbvtProxy*	links[2];
-int					stage;
+int				stage;
 /* ctor			*/ 
 btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
 	btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask)
@@ -71,9 +72,16 @@ btScalar				m_predictedframes;			// Frames predicted
 int						m_stageCurrent;				// Current stage
 int						m_fupdates;					// % of fixed updates per frame
 int						m_dupdates;					// % of dynamic updates per frame
+int						m_cupdates;					// % of cleanup updates per frame
+int						m_newpairs;					// Number of pairs created
+unsigned				m_updates_call;				// Number of updates call
+unsigned				m_updates_done;				// Number of updates done
+btScalar				m_updates_ratio;			// m_updates_done/m_updates_call
 int						m_pid;						// Parse id
+int						m_cid;						// Cleanup index
 int						m_gid;						// Gen id
 bool					m_releasepaircache;			// Release pair cache on delete
+bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
 #if DBVT_BP_PROFILE
 btClock					m_clock;
 struct	{
@@ -98,6 +106,7 @@ btOverlappingPairCache*			getOverlappingPairCache();
 const btOverlappingPairCache*	getOverlappingPairCache() const;
 void							getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
 void							printStats();
+static void						benchmark(btBroadphaseInterface*);
 };
 
 #endif