Merging trunk up to revision 41245.
[blender.git] / extern / bullet2 / src / BulletCollision / BroadphaseCollision / btDbvtBroadphase.cpp
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 {
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