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

Bullet Collision Detection & Physics Library

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