Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @ 8501

Last change on this file since 8501 was 8284, checked in by rgrieder, 14 years ago
Merged revisions 7978 - 8096 from kicklib to kicklib2.
Property svn:eol-style set to `native`
File size: 21.1 KB

Rev	Line
[1963]	1	/*
	2	Bullet Continuous Collision Detection and Physics Library
[8284]	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
[1963]	4
	5	This software is provided 'as-is', without any express or implied warranty.
	6	In no event will the authors be held liable for any damages arising from the use of this software.
	7	Permission is granted to anyone to use this software for any purpose,
	8	including commercial applications, and to alter it and redistribute it freely,
	9	subject to the following restrictions:
	10
	11	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.
	12	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
	13	3. This notice may not be removed or altered from any source distribution.
	14	*/
[8284]	15
[1963]	16	///btDbvtBroadphase implementation by Nathanael Presson
	17
	18	#include "btDbvtBroadphase.h"
	19
	20	//
	21	// Profiling
	22	//
	23
	24	#if DBVT_BP_PROFILE\|\|DBVT_BP_ENABLE_BENCHMARK
	25	#include <stdio.h>
	26	#endif
	27
	28	#if DBVT_BP_PROFILE
	29	struct ProfileScope
[2430]	30	{
	31	__forceinline ProfileScope(btClock& clock,unsigned long& value) :
	32	m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
[1972]	33	{
[2430]	34	}
[1963]	35	__forceinline ~ProfileScope()
[2430]	36	{
[1963]	37	(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
[2430]	38	}
[1963]	39	btClock* m_clock;
	40	unsigned long* m_value;
	41	unsigned long m_base;
[2430]	42	};
[1963]	43	#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_)
	44	#else
	45	#define SPC(_value_)
	46	#endif
	47
	48	//
	49	// Helpers
	50	//
	51
	52	//
	53	template <typename T>
	54	static inline void listappend(T* item,T*& list)
	55	{
[2430]	56	item->links[0]=0;
	57	item->links[1]=list;
	58	if(list) list->links[0]=item;
	59	list=item;
[1963]	60	}
	61
	62	//
	63	template <typename T>
	64	static inline void listremove(T* item,T*& list)
	65	{
[2430]	66	if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
	67	if(item->links[1]) item->links[1]->links[0]=item->links[0];
[1963]	68	}
	69
	70	//
	71	template <typename T>
	72	static inline int listcount(T* root)
	73	{
[2430]	74	int n=0;
	75	while(root) { ++n;root=root->links[1]; }
	76	return(n);
[1963]	77	}
	78
	79	//
	80	template <typename T>
	81	static inline void clear(T& value)
	82	{
[2430]	83	static const struct ZeroDummy : T {} zerodummy;
	84	value=zerodummy;
[1963]	85	}
	86
	87	//
	88	// Colliders
	89	//
	90
	91	/* Tree collider */
	92	struct btDbvtTreeCollider : btDbvt::ICollide
	93	{
[2430]	94	btDbvtBroadphase* pbp;
	95	btDbvtProxy* proxy;
	96	btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
	97	void Process(const btDbvtNode* na,const btDbvtNode* nb)
[1963]	98	{
[2430]	99	if(na!=nb)
[1963]	100	{
[2430]	101	btDbvtProxy* pa=(btDbvtProxy*)na->data;
	102	btDbvtProxy* pb=(btDbvtProxy*)nb->data;
	103	#if DBVT_BP_SORTPAIRS
[2882]	104	if(pa->m_uniqueId>pb->m_uniqueId)
	105	btSwap(pa,pb);
[2430]	106	#endif
	107	pbp->m_paircache->addOverlappingPair(pa,pb);
	108	++pbp->m_newpairs;
[1963]	109	}
	110	}
[2430]	111	void Process(const btDbvtNode* n)
[1963]	112	{
[2430]	113	Process(n,proxy->leaf);
[1963]	114	}
	115	};
	116
	117	//
	118	// btDbvtBroadphase
	119	//
	120
	121	//
	122	btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
	123	{
[2430]	124	m_deferedcollide = false;
	125	m_needcleanup = true;
	126	m_releasepaircache = (paircache!=0)?false:true;
[8284]	127	m_prediction = 0;
[2430]	128	m_stageCurrent = 0;
	129	m_fixedleft = 0;
	130	m_fupdates = 1;
	131	m_dupdates = 0;
	132	m_cupdates = 10;
	133	m_newpairs = 1;
	134	m_updates_call = 0;
	135	m_updates_done = 0;
	136	m_updates_ratio = 0;
[2882]	137	m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
[2430]	138	m_gid = 0;
	139	m_pid = 0;
	140	m_cid = 0;
	141	for(int i=0;i<=STAGECOUNT;++i)
[1963]	142	{
[2430]	143	m_stageRoots[i]=0;
[1963]	144	}
	145	#if DBVT_BP_PROFILE
[2430]	146	clear(m_profiling);
[1963]	147	#endif
	148	}
	149
	150	//
	151	btDbvtBroadphase::~btDbvtBroadphase()
	152	{
[2430]	153	if(m_releasepaircache)
	154	{
	155	m_paircache->~btOverlappingPairCache();
	156	btAlignedFree(m_paircache);
	157	}
[1963]	158	}
	159
	160	//
	161	btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin,
[2430]	162	const btVector3& aabbMax,
	163	int /shapeType/,
	164	void* userPtr,
	165	short int collisionFilterGroup,
	166	short int collisionFilterMask,
	167	btDispatcher* /dispatcher/,
	168	void* /multiSapProxy/)
[1963]	169	{
[2430]	170	btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr,
	171	collisionFilterGroup,
	172	collisionFilterMask);
	173
	174	btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
	175
	176	//bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
	177	proxy->stage = m_stageCurrent;
	178	proxy->m_uniqueId = ++m_gid;
	179	proxy->leaf = m_sets[0].insert(aabb,proxy);
	180	listappend(proxy,m_stageRoots[m_stageCurrent]);
	181	if(!m_deferedcollide)
[1963]	182	{
[2430]	183	btDbvtTreeCollider collider(this);
	184	collider.proxy=proxy;
	185	m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
	186	m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
[1963]	187	}
[2430]	188	return(proxy);
[1963]	189	}
	190
	191	//
	192	void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
[2430]	193	btDispatcher* dispatcher)
[1963]	194	{
[2430]	195	btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
	196	if(proxy->stage==STAGECOUNT)
	197	m_sets[1].remove(proxy->leaf);
[1963]	198	else
[2430]	199	m_sets[0].remove(proxy->leaf);
	200	listremove(proxy,m_stageRoots[proxy->stage]);
	201	m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
	202	btAlignedFree(proxy);
	203	m_needcleanup=true;
[1963]	204	}
	205
[2430]	206	void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
	207	{
	208	btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
	209	aabbMin = proxy->m_aabbMin;
	210	aabbMax = proxy->m_aabbMax;
	211	}
	212
[2882]	213	struct BroadphaseRayTester : btDbvt::ICollide
[2430]	214	{
[2882]	215	btBroadphaseRayCallback& m_rayCallback;
	216	BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
	217	:m_rayCallback(orgCallback)
[2430]	218	{
[2882]	219	}
	220	void Process(const btDbvtNode* leaf)
	221	{
	222	btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
	223	m_rayCallback.process(proxy);
	224	}
	225	};
[2430]	226
[2882]	227	void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
	228	{
[2430]	229	BroadphaseRayTester callback(rayCallback);
	230
	231	m_sets[0].rayTestInternal( m_sets[0].m_root,
	232	rayFrom,
	233	rayTo,
	234	rayCallback.m_rayDirectionInverse,
	235	rayCallback.m_signs,
	236	rayCallback.m_lambda_max,
	237	aabbMin,
	238	aabbMax,
	239	callback);
	240
	241	m_sets[1].rayTestInternal( m_sets[1].m_root,
	242	rayFrom,
	243	rayTo,
	244	rayCallback.m_rayDirectionInverse,
	245	rayCallback.m_signs,
	246	rayCallback.m_lambda_max,
	247	aabbMin,
	248	aabbMax,
	249	callback);
	250
	251	}
	252
[8284]	253
	254	struct BroadphaseAabbTester : btDbvt::ICollide
	255	{
	256	btBroadphaseAabbCallback& m_aabbCallback;
	257	BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
	258	:m_aabbCallback(orgCallback)
	259	{
	260	}
	261	void Process(const btDbvtNode* leaf)
	262	{
	263	btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
	264	m_aabbCallback.process(proxy);
	265	}
	266	};
	267
	268	void btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback)
	269	{
	270	BroadphaseAabbTester callback(aabbCallback);
	271
	272	const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax);
	273	//process all children, that overlap with the given AABB bounds
	274	m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
	275	m_sets[1].collideTV(m_sets[1].m_root,bounds,callback);
	276
	277	}
	278
	279
	280
[1963]	281	//
	282	void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
[2430]	283	const btVector3& aabbMin,
	284	const btVector3& aabbMax,
	285	btDispatcher* /dispatcher/)
[1963]	286	{
[2430]	287	btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
	288	ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
[1963]	289	#if DBVT_BP_PREVENTFALSEUPDATE
[2430]	290	if(NotEqual(aabb,proxy->leaf->volume))
[1963]	291	#endif
	292	{
[2430]	293	bool docollide=false;
	294	if(proxy->stage==STAGECOUNT)
[1963]	295	{/* fixed -> dynamic set */
[2430]	296	m_sets[1].remove(proxy->leaf);
	297	proxy->leaf=m_sets[0].insert(aabb,proxy);
	298	docollide=true;
[1963]	299	}
	300	else
	301	{/* dynamic set */
[2430]	302	++m_updates_call;
	303	if(Intersect(proxy->leaf->volume,aabb))
[1963]	304	{/* Moving */
[2430]	305
	306	const btVector3 delta=aabbMin-proxy->m_aabbMin;
	307	btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
	308	if(delta[0]<0) velocity[0]=-velocity[0];
	309	if(delta[1]<0) velocity[1]=-velocity[1];
	310	if(delta[2]<0) velocity[2]=-velocity[2];
	311	if (
	312	#ifdef DBVT_BP_MARGIN
	313	m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
	314	#else
	315	m_sets[0].update(proxy->leaf,aabb,velocity)
	316	#endif
	317	)
[1963]	318	{
[2430]	319	++m_updates_done;
	320	docollide=true;
[1963]	321	}
	322	}
	323	else
	324	{/* Teleporting */
[2430]	325	m_sets[0].update(proxy->leaf,aabb);
	326	++m_updates_done;
	327	docollide=true;
[1963]	328	}
	329	}
[2430]	330	listremove(proxy,m_stageRoots[proxy->stage]);
	331	proxy->m_aabbMin = aabbMin;
	332	proxy->m_aabbMax = aabbMax;
	333	proxy->stage = m_stageCurrent;
	334	listappend(proxy,m_stageRoots[m_stageCurrent]);
	335	if(docollide)
[1963]	336	{
[2430]	337	m_needcleanup=true;
	338	if(!m_deferedcollide)
[1963]	339	{
[2430]	340	btDbvtTreeCollider collider(this);
	341	m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
	342	m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
[1963]	343	}
	344	}
	345	}
	346	}
	347
[8284]	348
[1963]	349	//
[8284]	350	void btDbvtBroadphase::setAabbForceUpdate( btBroadphaseProxy* absproxy,
	351	const btVector3& aabbMin,
	352	const btVector3& aabbMax,
	353	btDispatcher* /dispatcher/)
	354	{
	355	btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
	356	ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
	357	bool docollide=false;
	358	if(proxy->stage==STAGECOUNT)
	359	{/* fixed -> dynamic set */
	360	m_sets[1].remove(proxy->leaf);
	361	proxy->leaf=m_sets[0].insert(aabb,proxy);
	362	docollide=true;
	363	}
	364	else
	365	{/* dynamic set */
	366	++m_updates_call;
	367	/* Teleporting */
	368	m_sets[0].update(proxy->leaf,aabb);
	369	++m_updates_done;
	370	docollide=true;
	371	}
	372	listremove(proxy,m_stageRoots[proxy->stage]);
	373	proxy->m_aabbMin = aabbMin;
	374	proxy->m_aabbMax = aabbMax;
	375	proxy->stage = m_stageCurrent;
	376	listappend(proxy,m_stageRoots[m_stageCurrent]);
	377	if(docollide)
	378	{
	379	m_needcleanup=true;
	380	if(!m_deferedcollide)
	381	{
	382	btDbvtTreeCollider collider(this);
	383	m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
	384	m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
	385	}
	386	}
	387	}
	388
	389	//
[1963]	390	void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
	391	{
[2430]	392	collide(dispatcher);
[1963]	393	#if DBVT_BP_PROFILE
[2430]	394	if(0==(m_pid%DBVT_BP_PROFILING_RATE))
[1963]	395	{
[2430]	396	printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
	397	unsigned int total=m_profiling.m_total;
	398	if(total<=0) total=1;
	399	printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
	400	printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
	401	printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
	402	printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE);
	403	const unsigned long sum=m_profiling.m_ddcollide+
	404	m_profiling.m_fdcollide+
	405	m_profiling.m_cleanup;
	406	printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
	407	printf("job counts: %u%%\r\n",(m_profiling.m_jobcount100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)DBVT_BP_PROFILING_RATE));
	408	clear(m_profiling);
	409	m_clock.reset();
[1963]	410	}
	411	#endif
[2882]	412
	413	performDeferredRemoval(dispatcher);
	414
[1963]	415	}
	416
[2882]	417	void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
	418	{
	419
	420	if (m_paircache->hasDeferredRemoval())
	421	{
	422
	423	btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
	424
	425	//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
	426	overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
	427
	428	int invalidPair = 0;
	429
	430
	431	int i;
	432
	433	btBroadphasePair previousPair;
	434	previousPair.m_pProxy0 = 0;
	435	previousPair.m_pProxy1 = 0;
	436	previousPair.m_algorithm = 0;
	437
	438
	439	for (i=0;i<overlappingPairArray.size();i++)
	440	{
	441
	442	btBroadphasePair& pair = overlappingPairArray[i];
	443
	444	bool isDuplicate = (pair == previousPair);
	445
	446	previousPair = pair;
	447
	448	bool needsRemoval = false;
	449
	450	if (!isDuplicate)
	451	{
	452	//important to perform AABB check that is consistent with the broadphase
	453	btDbvtProxy* pa=(btDbvtProxy*)pair.m_pProxy0;
	454	btDbvtProxy* pb=(btDbvtProxy*)pair.m_pProxy1;
	455	bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
	456
	457	if (hasOverlap)
	458	{
	459	needsRemoval = false;
	460	} else
	461	{
	462	needsRemoval = true;
	463	}
	464	} else
	465	{
	466	//remove duplicate
	467	needsRemoval = true;
	468	//should have no algorithm
	469	btAssert(!pair.m_algorithm);
	470	}
	471
	472	if (needsRemoval)
	473	{
	474	m_paircache->cleanOverlappingPair(pair,dispatcher);
	475
	476	pair.m_pProxy0 = 0;
	477	pair.m_pProxy1 = 0;
	478	invalidPair++;
	479	}
	480
	481	}
	482
	483	//perform a sort, to sort 'invalid' pairs to the end
	484	overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
	485	overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
	486	}
	487	}
	488
[1963]	489	//
	490	void btDbvtBroadphase::collide(btDispatcher* dispatcher)
	491	{
[2882]	492	/*printf("---------------------------------------------------------\n");
	493	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
	494	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
	495	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
	496	{
	497	int i;
	498	for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
	499	{
	500	printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
	501	getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
	502	}
	503	printf("\n");
	504	}
	505	*/
	506
	507
	508
[2430]	509	SPC(m_profiling.m_total);
	510	/* optimize */
	511	m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
	512	if(m_fixedleft)
[1963]	513	{
[2430]	514	const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
	515	m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
	516	m_fixedleft=btMax<int>(0,m_fixedleft-count);
[1963]	517	}
[2430]	518	/* dynamic -> fixed set */
	519	m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
	520	btDbvtProxy* current=m_stageRoots[m_stageCurrent];
	521	if(current)
[1963]	522	{
[2430]	523	btDbvtTreeCollider collider(this);
	524	do {
	525	btDbvtProxy* next=current->links[1];
	526	listremove(current,m_stageRoots[current->stage]);
	527	listappend(current,m_stageRoots[STAGECOUNT]);
	528	#if DBVT_BP_ACCURATESLEEPING
	529	m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
	530	collider.proxy=current;
	531	btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
	532	btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
	533	#endif
	534	m_sets[0].remove(current->leaf);
	535	ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
	536	current->leaf = m_sets[1].insert(curAabb,current);
	537	current->stage = STAGECOUNT;
	538	current = next;
[1963]	539	} while(current);
[2430]	540	m_fixedleft=m_sets[1].m_leaves;
	541	m_needcleanup=true;
[1963]	542	}
[2430]	543	/* collide dynamics */
[1963]	544	{
[2430]	545	btDbvtTreeCollider collider(this);
	546	if(m_deferedcollide)
[1963]	547	{
[2430]	548	SPC(m_profiling.m_fdcollide);
	549	m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
[1963]	550	}
[2430]	551	if(m_deferedcollide)
[1963]	552	{
[2430]	553	SPC(m_profiling.m_ddcollide);
	554	m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
[1963]	555	}
	556	}
[2430]	557	/* clean up */
	558	if(m_needcleanup)
[1963]	559	{
[2430]	560	SPC(m_profiling.m_cleanup);
	561	btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
	562	if(pairs.size()>0)
[1963]	563	{
[2882]	564
	565	int ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
[2430]	566	for(int i=0;i<ni;++i)
[1963]	567	{
[2882]	568	btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()];
[2430]	569	btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
	570	btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
	571	if(!Intersect(pa->leaf->volume,pb->leaf->volume))
[1963]	572	{
[2430]	573	#if DBVT_BP_SORTPAIRS
[2882]	574	if(pa->m_uniqueId>pb->m_uniqueId)
	575	btSwap(pa,pb);
[2430]	576	#endif
	577	m_paircache->removeOverlappingPair(pa,pb,dispatcher);
	578	--ni;--i;
[1963]	579	}
	580	}
[2430]	581	if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
[1963]	582	}
	583	}
[2430]	584	++m_pid;
	585	m_newpairs=1;
	586	m_needcleanup=false;
	587	if(m_updates_call>0)
[1963]	588	{ m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
	589	else
	590	{ m_updates_ratio=0; }
[2430]	591	m_updates_done/=2;
	592	m_updates_call/=2;
[1963]	593	}
	594
	595	//
	596	void btDbvtBroadphase::optimize()
	597	{
[2430]	598	m_sets[0].optimizeTopDown();
	599	m_sets[1].optimizeTopDown();
[1963]	600	}
	601
	602	//
	603	btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
	604	{
[2430]	605	return(m_paircache);
[1963]	606	}
	607
	608	//
	609	const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
	610	{
[2430]	611	return(m_paircache);
[1963]	612	}
	613
	614	//
	615	void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
	616	{
	617
	618	ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds;
	619
[2430]	620	if(!m_sets[0].empty())
	621	if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
	622	m_sets[1].m_root->volume,bounds);
	623	else
	624	bounds=m_sets[0].m_root->volume;
	625	else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
	626	else
	627	bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
	628	aabbMin=bounds.Mins();
	629	aabbMax=bounds.Maxs();
[1963]	630	}
	631
[2882]	632	void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
	633	{
	634
	635	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
	636	if (!totalObjects)
	637	{
	638	//reset internal dynamic tree data structures
	639	m_sets[0].clear();
	640	m_sets[1].clear();
	641
	642	m_deferedcollide = false;
	643	m_needcleanup = true;
	644	m_stageCurrent = 0;
	645	m_fixedleft = 0;
	646	m_fupdates = 1;
	647	m_dupdates = 0;
	648	m_cupdates = 10;
	649	m_newpairs = 1;
	650	m_updates_call = 0;
	651	m_updates_done = 0;
	652	m_updates_ratio = 0;
	653
	654	m_gid = 0;
	655	m_pid = 0;
	656	m_cid = 0;
	657	for(int i=0;i<=STAGECOUNT;++i)
	658	{
	659	m_stageRoots[i]=0;
	660	}
	661	}
	662	}
	663
[1963]	664	//
	665	void btDbvtBroadphase::printStats()
	666	{}
	667
	668	//
	669	#if DBVT_BP_ENABLE_BENCHMARK
	670
	671	struct btBroadphaseBenchmark
[2430]	672	{
	673	struct Experiment
[1972]	674	{
[1963]	675	const char* name;
	676	int object_count;
	677	int update_count;
	678	int spawn_count;
	679	int iterations;
	680	btScalar speed;
	681	btScalar amplitude;
[2430]	682	};
[1963]	683	struct Object
[2430]	684	{
[1963]	685	btVector3 center;
	686	btVector3 extents;
	687	btBroadphaseProxy* proxy;
	688	btScalar time;
	689	void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
[2430]	690	{
[1963]	691	time += speed;
	692	center[0] = btCos(time(btScalar)2.17)amplitude+
[2430]	693	btSin(time)*amplitude/2;
[1963]	694	center[1] = btCos(time(btScalar)1.38)amplitude+
[2430]	695	btSin(time)*amplitude;
[1963]	696	center[2] = btSin(time(btScalar)0.777)amplitude;
	697	pbi->setAabb(proxy,center-extents,center+extents,0);
[2430]	698	}
	699	};
[1963]	700	static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); }
	701	static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); }
	702	static void OutputTime(const char* name,btClock& c,unsigned count=0)
[2430]	703	{
[1963]	704	const unsigned long us=c.getTimeMicroseconds();
	705	const unsigned long ms=(us+500)/1000;
	706	const btScalar sec=us/(btScalar)(1000*1000);
	707	if(count>0)
	708	printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
[2430]	709	else
[1963]	710	printf("%s : %u us (%u ms)\r\n",name,us,ms);
[2430]	711	}
	712	};
[1963]	713
	714	void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
	715	{
[2430]	716	static const btBroadphaseBenchmark::Experiment experiments[]=
[1963]	717	{
[2430]	718	{"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
	719	/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
	720	{"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
[1963]	721	};
[2430]	722	static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]);
	723	btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects;
	724	btClock wallclock;
	725	/* Begin */
	726	for(int iexp=0;iexp<nexperiments;++iexp)
[1963]	727	{
[2430]	728	const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp];
	729	const int object_count=experiment.object_count;
	730	const int update_count=(object_count*experiment.update_count)/100;
	731	const int spawn_count=(object_count*experiment.spawn_count)/100;
	732	const btScalar speed=experiment.speed;
	733	const btScalar amplitude=experiment.amplitude;
	734	printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
	735	printf("\tObjects: %u\r\n",object_count);
	736	printf("\tUpdate: %u\r\n",update_count);
	737	printf("\tSpawn: %u\r\n",spawn_count);
	738	printf("\tSpeed: %f\r\n",speed);
	739	printf("\tAmplitude: %f\r\n",amplitude);
	740	srand(180673);
	741	/* Create objects */
	742	wallclock.reset();
	743	objects.reserve(object_count);
	744	for(int i=0;i<object_count;++i)
[1963]	745	{
[2430]	746	btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object();
	747	po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
	748	po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
	749	po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
	750	po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
	751	po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
	752	po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
	753	po->time=btBroadphaseBenchmark::UnitRand()*2000;
	754	po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
	755	objects.push_back(po);
[1963]	756	}
[2430]	757	btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
	758	/* First update */
	759	wallclock.reset();
	760	for(int i=0;i<objects.size();++i)
[1963]	761	{
[2430]	762	objects[i]->update(speed,amplitude,pbi);
[1963]	763	}
[2430]	764	btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
	765	/* Updates */
	766	wallclock.reset();
	767	for(int i=0;i<experiment.iterations;++i)
[1963]	768	{
[2430]	769	for(int j=0;j<update_count;++j)
[1963]	770	{
[2430]	771	objects[j]->update(speed,amplitude,pbi);
[1963]	772	}
[2430]	773	pbi->calculateOverlappingPairs(0);
[1963]	774	}
[2430]	775	btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
	776	/* Clean up */
	777	wallclock.reset();
	778	for(int i=0;i<objects.size();++i)
[1963]	779	{
[2430]	780	pbi->destroyProxy(objects[i]->proxy,0);
	781	delete objects[i];
[1963]	782	}
[2430]	783	objects.resize(0);
	784	btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
[1963]	785	}
	786
	787	}
	788	#else
	789	void btDbvtBroadphase::benchmark(btBroadphaseInterface*)
	790	{}
	791	#endif
	792
	793	#if DBVT_BP_PROFILE
	794	#undef SPC
	795	#endif
[2882]	796

Note: See TracBrowser for help on using the repository browser.

Download in other formats: