Bullet Collision Detection & Physics Library: btDbvtBroadphase.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btDbvtBroadphase.cpp
Go to the documentation of this file.
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
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 {
31 __forceinline ProfileScope(btClock& clock,unsigned long& value) :
32 m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
33 {
34 }
35 __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>
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>
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>
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>
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;
96 btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
97 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 }
111 void Process(const btDbvtNode* n)
112 {
113 Process(n,proxy->leaf);
114 }
115 };
116
117 //
118 // btDbvtBroadphase
119 //
120
121 //
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 //
151 btDbvtBroadphase::~btDbvtBroadphase()
152 {
153 if(m_releasepaircache)
154 {
155 m_paircache->~btOverlappingPairCache();
156 btAlignedFree(m_paircache);
157 }
158 }
159
160 //
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 //
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
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
213 struct BroadphaseRayTester : btDbvt::ICollide
214 {
215 btBroadphaseRayCallback& m_rayCallback;
216 BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
217 :m_rayCallback(orgCallback)
218 {
219 }
220 void Process(const btDbvtNode* leaf)
221 {
222 btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
223 m_rayCallback.process(proxy);
224 }
225 };
226
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;
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
281 //
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 //
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 //
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
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 //
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 //
596 void btDbvtBroadphase::optimize()
597 {
598 m_sets[0].optimizeTopDown();
599 m_sets[1].optimizeTopDown();
600 }
601
602 //
603 btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
604 {
605 return(m_paircache);
606 }
607
608 //
609 const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
610 {
611 return(m_paircache);
612 }
613
614 //
615 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
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 //
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;
689 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 };
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)
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
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
789 void btDbvtBroadphase::benchmark(btBroadphaseInterface*)
790 {}
791 #endif
792
793 #if DBVT_BP_PROFILE
794 #undef SPC
795 #endif
796
btDbvtBroadphase::aabbTest
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
Definition: btDbvtBroadphase.cpp:268
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:300
btBroadphaseRayCallback::m_lambda_max
btScalar m_lambda_max
Definition: btBroadphaseInterface.h:41
Merge
DBVT_INLINE void Merge(const btDbvtAabbMm &a, const btDbvtAabbMm &b, btDbvtAabbMm &r)
Definition: btDbvt.h:653
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:276
btDbvtBroadphase::createProxy
btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, short int collisionFilterGroup, short int collisionFilterMask, btDispatcher *dispatcher, void *multiSapProxy)
Definition: btDbvtBroadphase.cpp:161
btDbvtBroadphase::STAGECOUNT
Definition: btDbvtBroadphase.h:68
btDbvtBroadphase::benchmark
static void benchmark(btBroadphaseInterface *)
Definition: btDbvtBroadphase.cpp:789
btDbvtProxy::stage
int stage
Definition: btDbvtBroadphase.h:48
btOverlappingPairCache::cleanOverlappingPair
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
btDbvtBroadphase::m_cupdates
int m_cupdates
Definition: btDbvtBroadphase.h:78
BroadphaseRayTester::m_rayCallback
btBroadphaseRayCallback & m_rayCallback
Definition: btDbvtBroadphase.cpp:215
listappend
static void listappend(T *item, T *&list)
Definition: btDbvtBroadphase.cpp:54
btDbvtBroadphase::resetPool
virtual void resetPool(btDispatcher *dispatcher)
reset broadphase internal structures, to ensure determinism/reproducability
Definition: btDbvtBroadphase.cpp:632
btDbvtBroadphase::m_deferedcollide
bool m_deferedcollide
Definition: btDbvtBroadphase.h:88
btAlignedObjectArray< btBroadphasePair >
btDbvtBroadphase.h
btDbvtAabbMm
Definition: btDbvt.h:130
btOverlappingPairCache::hasDeferredRemoval
virtual bool hasDeferredRemoval()=0
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:451
btBroadphaseProxy::m_aabbMax
btVector3 m_aabbMax
Definition: btBroadphaseProxy.h:110
btDbvtNode::data
void * data
Definition: btDbvt.h:186
btDbvtBroadphase::m_paircache
btOverlappingPairCache * m_paircache
Definition: btDbvtBroadphase.h:73
btDbvtBroadphase::m_fupdates
int m_fupdates
Definition: btDbvtBroadphase.h:76
btDbvtTreeCollider::Process
void Process(const btDbvtNode *na, const btDbvtNode *nb)
Definition: btDbvtBroadphase.cpp:97
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btDbvtProxy
Definition: btDbvtBroadphase.h:42
btDbvt::rayTestInternal
DBVT_PREFIX void rayTestInternal(const btDbvtNode *root, const btVector3 &rayFrom, const btVector3 &rayTo, const btVector3 &rayDirectionInverse, unsigned int signs[3], btScalar lambda_max, const btVector3 &aabbMin, const btVector3 &aabbMax, DBVT_IPOLICY) const
rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory ...
Definition: btDbvt.h:954
btDbvt::insert
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:485
btClock::getTimeMicroseconds
unsigned long int getTimeMicroseconds()
Returns the time in us since the last call to reset or since the Clock was created.
Definition: btQuickprof.cpp:187
btClock
The btClock is a portable basic clock that measures accurate time in seconds, use for profiling...
Definition: btQuickprof.h:35
btDbvtBroadphase
The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees...
Definition: btDbvtBroadphase.h:62
btDbvt::m_root
btDbvtNode * m_root
Definition: btDbvt.h:258
BroadphaseAabbTester
Definition: btDbvtBroadphase.cpp:254
btClock::reset
void reset()
Resets the initial reference time.
Definition: btQuickprof.cpp:109
btDbvtBroadphase::performDeferredRemoval
void performDeferredRemoval(btDispatcher *dispatcher)
Definition: btDbvtBroadphase.cpp:417
btDbvtBroadphase::m_fixedleft
int m_fixedleft
Definition: btDbvtBroadphase.h:80
btDbvtBroadphase::m_needcleanup
bool m_needcleanup
Definition: btDbvtBroadphase.h:89
btDbvtBroadphase::collide
void collide(btDispatcher *dispatcher)
Definition: btDbvtBroadphase.cpp:490
btDbvtBroadphase::m_stageCurrent
int m_stageCurrent
Definition: btDbvtBroadphase.h:75
btDbvtProxy::leaf
btDbvtNode * leaf
Definition: btDbvtBroadphase.h:46
btDbvtProxy::links
btDbvtProxy * links[2]
Definition: btDbvtBroadphase.h:47
BroadphaseRayTester
Definition: btDbvtBroadphase.cpp:213
DBVT_BP_MARGIN
#define DBVT_BP_MARGIN
Definition: btDbvtBroadphase.h:32
btOverlappingPairCache
The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
Definition: btOverlappingPairCache.h:60
BroadphaseRayTester::Process
void Process(const btDbvtNode *leaf)
Definition: btDbvtBroadphase.cpp:220
btDbvt::update
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:494
btDbvtBroadphase::m_stageRoots
btDbvtProxy * m_stageRoots[STAGECOUNT+1]
Definition: btDbvtBroadphase.h:72
btDbvtBroadphase::btDbvtBroadphase
btDbvtBroadphase(btOverlappingPairCache *paircache=0)
Definition: btDbvtBroadphase.cpp:122
btDbvtBroadphase::m_gid
int m_gid
Definition: btDbvtBroadphase.h:86
btDbvtTreeCollider::Process
void Process(const btDbvtNode *n)
Definition: btDbvtBroadphase.cpp:111
btDbvtTreeCollider::proxy
btDbvtProxy * proxy
Definition: btDbvtBroadphase.cpp:95
btDbvt::clear
void clear()
Definition: btDbvt.cpp:427
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btDbvtBroadphase::setAabb
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
Definition: btDbvtBroadphase.cpp:282
btDbvt::optimizeIncremental
void optimizeIncremental(int passes)
Definition: btDbvt.cpp:465
btOverlappingPairCache::getOverlappingPairArray
virtual btBroadphasePairArray & getOverlappingPairArray()=0
btDbvtBroadphase::m_cid
int m_cid
Definition: btDbvtBroadphase.h:85
SPC
#define SPC(_value_)
btDbvtBroadphase implementation by Nathanael Presson
Definition: btDbvtBroadphase.cpp:45
btDbvtBroadphase::getOverlappingPairCache
virtual btOverlappingPairCache * getOverlappingPairCache()
Definition: btDbvtBroadphase.cpp:603
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:585
btDbvtAabbMm::FromMM
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:411
btDbvtBroadphase::optimize
void optimize()
Definition: btDbvtBroadphase.cpp:596
btDbvtBroadphase::destroyProxy
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
Definition: btDbvtBroadphase.cpp:192
btDbvtBroadphase::rayTest
virtual void rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, btBroadphaseRayCallback &rayCallback, const btVector3 &aabbMin=btVector3(0, 0, 0), const btVector3 &aabbMax=btVector3(0, 0, 0))
Definition: btDbvtBroadphase.cpp:227
btBroadphaseInterface::createProxy
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, short int collisionFilterGroup, short int collisionFilterMask, btDispatcher *dispatcher, void *multiSapProxy)=0
listremove
static void listremove(T *item, T *&list)
Definition: btDbvtBroadphase.cpp:64
btBroadphaseRayCallback
Definition: btBroadphaseInterface.h:36
btAlignedFree
#define btAlignedFree(ptr)
Definition: btAlignedAllocator.h:42
listcount
static int listcount(T *root)
Definition: btDbvtBroadphase.cpp:72
btDbvtBroadphase::printStats
virtual void printStats()
Definition: btDbvtBroadphase.cpp:665
btOverlappingPairCallback::removeOverlappingPairsContainingProxy
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
btDbvtAabbMm::FromCR
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
Definition: btDbvt.h:405
btBroadphaseInterface
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs...
Definition: btBroadphaseInterface.h:51
btOverlappingPairCache::getNumOverlappingPairs
virtual int getNumOverlappingPairs() const =0
btDbvt::optimizeTopDown
void optimizeTopDown(int bu_treshold=128)
Definition: btDbvt.cpp:453
btOverlappingPairCache::~btOverlappingPairCache
virtual ~btOverlappingPairCache()
Definition: btOverlappingPairCache.h:63
btBroadphaseProxy
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
Definition: btBroadphaseProxy.h:85
btBroadphasePair::m_pProxy1
btBroadphaseProxy * m_pProxy1
Definition: btBroadphaseProxy.h:227
btBroadphasePair::m_algorithm
btCollisionAlgorithm * m_algorithm
Definition: btBroadphaseProxy.h:229
btDbvtBroadphase::m_newpairs
int m_newpairs
Definition: btDbvtBroadphase.h:79
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:64
btDbvt::collideTTpersistentStack
DBVT_PREFIX void collideTTpersistentStack(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:789
btBroadphaseAabbCallback::process
virtual bool process(const btBroadphaseProxy *proxy)=0
btDbvtAabbMm::Maxs
DBVT_INLINE const btVector3 & Maxs() const
Definition: btDbvt.h:136
BroadphaseAabbTester::m_aabbCallback
btBroadphaseAabbCallback & m_aabbCallback
Definition: btDbvtBroadphase.cpp:256
btBroadphasePair::m_pProxy0
btBroadphaseProxy * m_pProxy0
Definition: btBroadphaseProxy.h:226
btDbvtBroadphase::setAabbForceUpdate
void setAabbForceUpdate(btBroadphaseProxy *absproxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *)
this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
Definition: btDbvtBroadphase.cpp:350
btDbvt::ICollide
Definition: btDbvt.h:227
btBroadphaseProxy::m_aabbMin
btVector3 m_aabbMin
Definition: btBroadphaseProxy.h:109
btDbvtTreeCollider::btDbvtTreeCollider
btDbvtTreeCollider(btDbvtBroadphase *p)
Definition: btDbvtBroadphase.cpp:96
BroadphaseRayTester::BroadphaseRayTester
BroadphaseRayTester(btBroadphaseRayCallback &orgCallback)
Definition: btDbvtBroadphase.cpp:216
btOverlappingPairCallback::addOverlappingPair
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
btDbvtBroadphase::m_updates_call
unsigned m_updates_call
Definition: btDbvtBroadphase.h:81
clear
static void clear(T &value)
Definition: btDbvtBroadphase.cpp:81
btDbvtBroadphase::m_sets
btDbvt m_sets[2]
Definition: btDbvtBroadphase.h:71
btBroadphaseRayCallback::m_signs
unsigned int m_signs[3]
Definition: btBroadphaseInterface.h:40
btDbvtBroadphase::getBroadphaseAabb
virtual void getBroadphaseAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the 'global' coordinate frame will add some transfor...
Definition: btDbvtBroadphase.cpp:615
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:220
btDbvtNode::volume
btDbvtVolume volume
Definition: btDbvt.h:179
btDbvtBroadphase::m_pid
int m_pid
Definition: btDbvtBroadphase.h:84
btBroadphasePairSortPredicate
Definition: btBroadphaseProxy.h:245
btBroadphaseProxy::m_uniqueId
int m_uniqueId
Definition: btBroadphaseProxy.h:107
btDbvtBroadphase::m_updates_ratio
btScalar m_updates_ratio
Definition: btDbvtBroadphase.h:83
btBroadphaseRayCallback::m_rayDirectionInverse
btVector3 m_rayDirectionInverse
added some cached data to accelerate ray-AABB tests
Definition: btBroadphaseInterface.h:39
Intersect
DBVT_INLINE bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:520
btBroadphaseInterface::setAabb
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)=0
btAlignedAlloc
#define btAlignedAlloc(size, alignment)
Definition: btAlignedAllocator.h:41
btDbvtBroadphase::~btDbvtBroadphase
~btDbvtBroadphase()
Definition: btDbvtBroadphase.cpp:151
btDbvtBroadphase::calculateOverlappingPairs
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
Definition: btDbvtBroadphase.cpp:390
btDbvtTreeCollider
Definition: btDbvtBroadphase.cpp:92
btDbvtNode
Definition: btDbvt.h:177
btDbvtBroadphase::getAabb
virtual void getAabb(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
Definition: btDbvtBroadphase.cpp:206
btDbvtTreeCollider::pbp
btDbvtBroadphase * pbp
Definition: btDbvtBroadphase.cpp:94
btBroadphaseAabbCallback
Definition: btBroadphaseInterface.h:29
btDbvtAabbMm::Mins
DBVT_INLINE const btVector3 & Mins() const
Definition: btDbvt.h:135
btDbvtBroadphase::m_updates_done
unsigned m_updates_done
Definition: btDbvtBroadphase.h:82
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
btDbvtBroadphase::m_releasepaircache
bool m_releasepaircache
Definition: btDbvtBroadphase.h:87
btDbvt::collideTV
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:922
BroadphaseAabbTester::BroadphaseAabbTester
BroadphaseAabbTester(btBroadphaseAabbCallback &orgCallback)
Definition: btDbvtBroadphase.cpp:257
btBroadphaseInterface::destroyProxy
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)=0
btHashedOverlappingPairCache
Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman...
Definition: btOverlappingPairCache.h:93
btDbvt::remove
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:557
btDbvtBroadphase::m_dupdates
int m_dupdates
Definition: btDbvtBroadphase.h:77
btBroadphaseInterface::calculateOverlappingPairs
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)=0
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:365
btOverlappingPairCallback::removeOverlappingPair
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
btDbvt::m_leaves
int m_leaves
Definition: btDbvt.h:261
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:450
bounds
static btDbvtVolume bounds(const tNodeArray &leaves)
Definition: btDbvt.cpp:250
NotEqual
DBVT_INLINE bool NotEqual(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:676
BroadphaseAabbTester::Process
void Process(const btDbvtNode *leaf)
Definition: btDbvtBroadphase.cpp:261
btDbvtBroadphase::m_prediction
btScalar m_prediction
Definition: btDbvtBroadphase.h:74
btBroadphasePair
The btBroadphasePair class contains a pair of aabb-overlapping objects.
Definition: btBroadphaseProxy.h:186