1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
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 */
15
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
30 {
ProfileScopeProfileScope31 __forceinline ProfileScope(btClock& clock,unsigned long& value) :
32 m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
33 {
34 }
~ProfileScopeProfileScope35 __forceinline ~ProfileScope()
36 {
37 (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
38 }
39 btClock* m_clock;
40 unsigned long* m_value;
41 unsigned long m_base;
42 };
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>
listappend(T * item,T * & list)54 static inline void listappend(T* item,T*& list)
55 {
56 item->links[0]=0;
57 item->links[1]=list;
58 if(list) list->links[0]=item;
59 list=item;
60 }
61
62 //
63 template <typename T>
listremove(T * item,T * & list)64 static inline void listremove(T* item,T*& list)
65 {
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];
68 }
69
70 //
71 template <typename T>
listcount(T * root)72 static inline int listcount(T* root)
73 {
74 int n=0;
75 while(root) { ++n;root=root->links[1]; }
76 return(n);
77 }
78
79 //
80 template <typename T>
clear(T & value)81 static inline void clear(T& value)
82 {
83 static const struct ZeroDummy : T {} zerodummy;
84 value=zerodummy;
85 }
86
87 //
88 // Colliders
89 //
90
91 /* Tree collider */
92 struct btDbvtTreeCollider : btDbvt::ICollide
93 {
94 btDbvtBroadphase* pbp;
95 btDbvtProxy* proxy;
btDbvtTreeColliderbtDbvtTreeCollider96 btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
ProcessbtDbvtTreeCollider97 void Process(const btDbvtNode* na,const btDbvtNode* nb)
98 {
99 if(na!=nb)
100 {
101 btDbvtProxy* pa=(btDbvtProxy*)na->data;
102 btDbvtProxy* pb=(btDbvtProxy*)nb->data;
103 #if DBVT_BP_SORTPAIRS
104 if(pa->m_uniqueId>pb->m_uniqueId)
105 btSwap(pa,pb);
106 #endif
107 pbp->m_paircache->addOverlappingPair(pa,pb);
108 ++pbp->m_newpairs;
109 }
110 }
ProcessbtDbvtTreeCollider111 void Process(const btDbvtNode* n)
112 {
113 Process(n,proxy->leaf);
114 }
115 };
116
117 //
118 // btDbvtBroadphase
119 //
120
121 //
btDbvtBroadphase(btOverlappingPairCache * paircache)122 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
123 {
124 m_deferedcollide = false;
125 m_needcleanup = true;
126 m_releasepaircache = (paircache!=0)?false:true;
127 m_prediction = 0;
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;
137 m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
138 m_gid = 0;
139 m_pid = 0;
140 m_cid = 0;
141 for(int i=0;i<=STAGECOUNT;++i)
142 {
143 m_stageRoots[i]=0;
144 }
145 #if DBVT_BP_PROFILE
146 clear(m_profiling);
147 #endif
148 }
149
150 //
~btDbvtBroadphase()151 btDbvtBroadphase::~btDbvtBroadphase()
152 {
153 if(m_releasepaircache)
154 {
155 m_paircache->~btOverlappingPairCache();
156 btAlignedFree(m_paircache);
157 }
158 }
159
160 //
createProxy(const btVector3 & aabbMin,const btVector3 & aabbMax,int,void * userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher *,void *)161 btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin,
162 const btVector3& aabbMax,
163 int /*shapeType*/,
164 void* userPtr,
165 short int collisionFilterGroup,
166 short int collisionFilterMask,
167 btDispatcher* /*dispatcher*/,
168 void* /*multiSapProxy*/)
169 {
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)
182 {
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);
187 }
188 return(proxy);
189 }
190
191 //
destroyProxy(btBroadphaseProxy * absproxy,btDispatcher * dispatcher)192 void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
193 btDispatcher* dispatcher)
194 {
195 btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
196 if(proxy->stage==STAGECOUNT)
197 m_sets[1].remove(proxy->leaf);
198 else
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;
204 }
205
getAabb(btBroadphaseProxy * absproxy,btVector3 & aabbMin,btVector3 & aabbMax) const206 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
213 struct BroadphaseRayTester : btDbvt::ICollide
214 {
215 btBroadphaseRayCallback& m_rayCallback;
BroadphaseRayTesterBroadphaseRayTester216 BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
217 :m_rayCallback(orgCallback)
218 {
219 }
ProcessBroadphaseRayTester220 void Process(const btDbvtNode* leaf)
221 {
222 btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
223 m_rayCallback.process(proxy);
224 }
225 };
226
rayTest(const btVector3 & rayFrom,const btVector3 & rayTo,btBroadphaseRayCallback & rayCallback,const btVector3 & aabbMin,const btVector3 & aabbMax)227 void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
228 {
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
253
254 struct BroadphaseAabbTester : btDbvt::ICollide
255 {
256 btBroadphaseAabbCallback& m_aabbCallback;
BroadphaseAabbTesterBroadphaseAabbTester257 BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
258 :m_aabbCallback(orgCallback)
259 {
260 }
ProcessBroadphaseAabbTester261 void Process(const btDbvtNode* leaf)
262 {
263 btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
264 m_aabbCallback.process(proxy);
265 }
266 };
267
aabbTest(const btVector3 & aabbMin,const btVector3 & aabbMax,btBroadphaseAabbCallback & aabbCallback)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
281 //
setAabb(btBroadphaseProxy * absproxy,const btVector3 & aabbMin,const btVector3 & aabbMax,btDispatcher *)282 void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
283 const btVector3& aabbMin,
284 const btVector3& aabbMax,
285 btDispatcher* /*dispatcher*/)
286 {
287 btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
288 ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
289 #if DBVT_BP_PREVENTFALSEUPDATE
290 if(NotEqual(aabb,proxy->leaf->volume))
291 #endif
292 {
293 bool docollide=false;
294 if(proxy->stage==STAGECOUNT)
295 {/* fixed -> dynamic set */
296 m_sets[1].remove(proxy->leaf);
297 proxy->leaf=m_sets[0].insert(aabb,proxy);
298 docollide=true;
299 }
300 else
301 {/* dynamic set */
302 ++m_updates_call;
303 if(Intersect(proxy->leaf->volume,aabb))
304 {/* Moving */
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 )
318 {
319 ++m_updates_done;
320 docollide=true;
321 }
322 }
323 else
324 {/* Teleporting */
325 m_sets[0].update(proxy->leaf,aabb);
326 ++m_updates_done;
327 docollide=true;
328 }
329 }
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)
336 {
337 m_needcleanup=true;
338 if(!m_deferedcollide)
339 {
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);
343 }
344 }
345 }
346 }
347
348
349 //
setAabbForceUpdate(btBroadphaseProxy * absproxy,const btVector3 & aabbMin,const btVector3 & aabbMax,btDispatcher *)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 //
calculateOverlappingPairs(btDispatcher * dispatcher)390 void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
391 {
392 collide(dispatcher);
393 #if DBVT_BP_PROFILE
394 if(0==(m_pid%DBVT_BP_PROFILING_RATE))
395 {
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_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
408 clear(m_profiling);
409 m_clock.reset();
410 }
411 #endif
412
413 performDeferredRemoval(dispatcher);
414
415 }
416
performDeferredRemoval(btDispatcher * dispatcher)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
489 //
collide(btDispatcher * dispatcher)490 void btDbvtBroadphase::collide(btDispatcher* dispatcher)
491 {
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
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)
513 {
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);
517 }
518 /* dynamic -> fixed set */
519 m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
520 btDbvtProxy* current=m_stageRoots[m_stageCurrent];
521 if(current)
522 {
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;
539 } while(current);
540 m_fixedleft=m_sets[1].m_leaves;
541 m_needcleanup=true;
542 }
543 /* collide dynamics */
544 {
545 btDbvtTreeCollider collider(this);
546 if(m_deferedcollide)
547 {
548 SPC(m_profiling.m_fdcollide);
549 m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
550 }
551 if(m_deferedcollide)
552 {
553 SPC(m_profiling.m_ddcollide);
554 m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
555 }
556 }
557 /* clean up */
558 if(m_needcleanup)
559 {
560 SPC(m_profiling.m_cleanup);
561 btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
562 if(pairs.size()>0)
563 {
564
565 int ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
566 for(int i=0;i<ni;++i)
567 {
568 btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()];
569 btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
570 btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
571 if(!Intersect(pa->leaf->volume,pb->leaf->volume))
572 {
573 #if DBVT_BP_SORTPAIRS
574 if(pa->m_uniqueId>pb->m_uniqueId)
575 btSwap(pa,pb);
576 #endif
577 m_paircache->removeOverlappingPair(pa,pb,dispatcher);
578 --ni;--i;
579 }
580 }
581 if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
582 }
583 }
584 ++m_pid;
585 m_newpairs=1;
586 m_needcleanup=false;
587 if(m_updates_call>0)
588 { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
589 else
590 { m_updates_ratio=0; }
591 m_updates_done/=2;
592 m_updates_call/=2;
593 }
594
595 //
optimize()596 void btDbvtBroadphase::optimize()
597 {
598 m_sets[0].optimizeTopDown();
599 m_sets[1].optimizeTopDown();
600 }
601
602 //
getOverlappingPairCache()603 btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
604 {
605 return(m_paircache);
606 }
607
608 //
getOverlappingPairCache() const609 const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
610 {
611 return(m_paircache);
612 }
613
614 //
getBroadphaseAabb(btVector3 & aabbMin,btVector3 & aabbMax) const615 void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
616 {
617
618 ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds;
619
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();
630 }
631
resetPool(btDispatcher * dispatcher)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
664 //
printStats()665 void btDbvtBroadphase::printStats()
666 {}
667
668 //
669 #if DBVT_BP_ENABLE_BENCHMARK
670
671 struct btBroadphaseBenchmark
672 {
673 struct Experiment
674 {
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;
682 };
683 struct Object
684 {
685 btVector3 center;
686 btVector3 extents;
687 btBroadphaseProxy* proxy;
688 btScalar time;
updatebtBroadphaseBenchmark::Object689 void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
690 {
691 time += speed;
692 center[0] = btCos(time*(btScalar)2.17)*amplitude+
693 btSin(time)*amplitude/2;
694 center[1] = btCos(time*(btScalar)1.38)*amplitude+
695 btSin(time)*amplitude;
696 center[2] = btSin(time*(btScalar)0.777)*amplitude;
697 pbi->setAabb(proxy,center-extents,center+extents,0);
698 }
699 };
UnsignedRandbtBroadphaseBenchmark700 static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); }
UnitRandbtBroadphaseBenchmark701 static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); }
OutputTimebtBroadphaseBenchmark702 static void OutputTime(const char* name,btClock& c,unsigned count=0)
703 {
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);
709 else
710 printf("%s : %u us (%u ms)\r\n",name,us,ms);
711 }
712 };
713
benchmark(btBroadphaseInterface * pbi)714 void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
715 {
716 static const btBroadphaseBenchmark::Experiment experiments[]=
717 {
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},*/
721 };
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)
727 {
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)
745 {
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);
756 }
757 btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
758 /* First update */
759 wallclock.reset();
760 for(int i=0;i<objects.size();++i)
761 {
762 objects[i]->update(speed,amplitude,pbi);
763 }
764 btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
765 /* Updates */
766 wallclock.reset();
767 for(int i=0;i<experiment.iterations;++i)
768 {
769 for(int j=0;j<update_count;++j)
770 {
771 objects[j]->update(speed,amplitude,pbi);
772 }
773 pbi->calculateOverlappingPairs(0);
774 }
775 btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
776 /* Clean up */
777 wallclock.reset();
778 for(int i=0;i<objects.size();++i)
779 {
780 pbi->destroyProxy(objects[i]->proxy,0);
781 delete objects[i];
782 }
783 objects.resize(0);
784 btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
785 }
786
787 }
788 #else
benchmark(btBroadphaseInterface *)789 void btDbvtBroadphase::benchmark(btBroadphaseInterface*)
790 {}
791 #endif
792
793 #if DBVT_BP_PROFILE
794 #undef SPC
795 #endif
796
797