Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @ 2119

Last change on this file since 2119 was 1972, checked in by rgrieder, 16 years ago

Downgraded Bullet to latest tagged version: 2.72
That should give us more stability.
  • Property svn:eol-style set to native
File size: 14.9 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15///btDbvtBroadphase implementation by Nathanael Presson
16
17#include "btDbvtBroadphase.h"
18
19//
20// Profiling
21//
22
23#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
24#include <stdio.h>
25#endif
26
27#if DBVT_BP_PROFILE
28struct  ProfileScope
29        {
30        __forceinline ProfileScope(btClock& clock,unsigned long& value) :
31                m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
32                {
33                }
34        __forceinline ~ProfileScope()
35                {
36                (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
37                }
38        btClock*                m_clock;
39        unsigned long*  m_value;
40        unsigned long   m_base;
41        };
42#define SPC(_value_)    ProfileScope    spc_scope(m_clock,_value_)
43#else
44#define SPC(_value_)
45#endif
46
47//
48// Helpers
49//
50
51//
52template <typename T>
53static inline void      listappend(T* item,T*& list)
54{
55item->links[0]=0;
56item->links[1]=list;
57if(list) list->links[0]=item;
58list=item;
59}
60
61//
62template <typename T>
63static inline void      listremove(T* item,T*& list)
64{
65if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
66if(item->links[1]) item->links[1]->links[0]=item->links[0];
67}
68
69//
70template <typename T>
71static inline int       listcount(T* root)
72{
73int     n=0;
74while(root) { ++n;root=root->links[1]; }
75return(n);
76}
77
78//
79template <typename T>
80static inline void      clear(T& value)
81{
82static const struct ZeroDummy : T {} zerodummy;
83value=zerodummy;
84}
85
86//
87// Colliders
88//
89
90/* Tree collider        */ 
91struct  btDbvtTreeCollider : btDbvt::ICollide
92{
93btDbvtBroadphase*       pbp;
94btDbvtProxy*            proxy;
95                btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
96void    Process(const btDbvtNode* na,const btDbvtNode* nb)
97        {
98        if(na!=nb)
99                {
100                btDbvtProxy*    pa=(btDbvtProxy*)na->data;
101                btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
102                #if DBVT_BP_SORTPAIRS
103                if(pa>pb) btSwap(pa,pb);
104                #endif
105                pbp->m_paircache->addOverlappingPair(pa,pb);
106                ++pbp->m_newpairs;
107                }
108        }
109void    Process(const btDbvtNode* n)
110        {
111        Process(n,proxy->leaf);
112        }
113};
114
115//
116// btDbvtBroadphase
117//
118
119//
120btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
121{
122m_deferedcollide        =       false;
123m_needcleanup           =       true;
124m_releasepaircache      =       (paircache!=0)?false:true;
125m_prediction            =       1/(btScalar)2;
126m_stageCurrent          =       0;
127m_fixedleft                     =       0;
128m_fupdates                      =       1;
129m_dupdates                      =       0;
130m_cupdates                      =       10;
131m_newpairs                      =       1;
132m_updates_call          =       0;
133m_updates_done          =       0;
134m_updates_ratio         =       0;
135m_paircache                     =       paircache?
136                                                paircache       :
137                                                new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
138m_gid                           =       0;
139m_pid                           =       0;
140m_cid                           =       0;
141for(int i=0;i<=STAGECOUNT;++i)
142        {
143        m_stageRoots[i]=0;
144        }
145#if DBVT_BP_PROFILE
146clear(m_profiling);
147#endif
148}
149
150//
151btDbvtBroadphase::~btDbvtBroadphase()
152{
153if(m_releasepaircache) 
154{
155        m_paircache->~btOverlappingPairCache();
156        btAlignedFree(m_paircache);
157}
158}
159
160//
161btBroadphaseProxy*                              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{
170btDbvtProxy*            proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  userPtr,
171                                                                                                                                                                        collisionFilterGroup,
172                                                                                                                                                                        collisionFilterMask);
173proxy->aabb                     =       btDbvtVolume::FromMM(aabbMin,aabbMax);
174proxy->stage            =       m_stageCurrent;
175proxy->m_uniqueId       =       ++m_gid;
176proxy->leaf                     =       m_sets[0].insert(proxy->aabb,proxy);
177listappend(proxy,m_stageRoots[m_stageCurrent]);
178if(!m_deferedcollide)
179        {
180        btDbvtTreeCollider      collider(this);
181        collider.proxy=proxy;
182        btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider);
183        btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider);
184        }
185return(proxy);
186}
187
188//
189void                                                    btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
190                                                                                                                                btDispatcher* dispatcher)
191{
192btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
193if(proxy->stage==STAGECOUNT)
194        m_sets[1].remove(proxy->leaf);
195        else
196        m_sets[0].remove(proxy->leaf);
197listremove(proxy,m_stageRoots[proxy->stage]);
198m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
199btAlignedFree(proxy);
200m_needcleanup=true;
201}
202
203//
204void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
205                                                                                                                                const btVector3& aabbMin,
206                                                                                                                                const btVector3& aabbMax,
207                                                                                                                                btDispatcher* /*dispatcher*/)
208{
209btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
210ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
211#if DBVT_BP_PREVENTFALSEUPDATE
212if(NotEqual(aabb,proxy->leaf->volume))
213#endif
214        {
215        bool    docollide=false;
216        if(proxy->stage==STAGECOUNT)
217                {/* fixed -> dynamic set        */ 
218                m_sets[1].remove(proxy->leaf);
219                proxy->leaf=m_sets[0].insert(aabb,proxy);
220                docollide=true;
221                }
222                else
223                {/* dynamic set                         */ 
224                ++m_updates_call;
225                if(Intersect(proxy->leaf->volume,aabb))
226                        {/* Moving                              */ 
227                        const btVector3 delta=aabbMin-proxy->aabb.Mins();
228                        btVector3               velocity(aabb.Extents()*m_prediction);
229                        if(delta[0]<0) velocity[0]=-velocity[0];
230                        if(delta[1]<0) velocity[1]=-velocity[1];
231                        if(delta[2]<0) velocity[2]=-velocity[2];
232                        if      (
233                                #ifdef DBVT_BP_MARGIN                           
234                                m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
235                                #else
236                                m_sets[0].update(proxy->leaf,aabb,velocity)
237                                #endif
238                                )
239                                {
240                                ++m_updates_done;
241                                docollide=true;
242                                }
243                        }
244                        else
245                        {/* Teleporting                 */ 
246                        m_sets[0].update(proxy->leaf,aabb);
247                        ++m_updates_done;
248                        docollide=true;
249                        }       
250                }
251        listremove(proxy,m_stageRoots[proxy->stage]);
252        proxy->aabb             =       aabb;
253        proxy->stage    =       m_stageCurrent;
254        listappend(proxy,m_stageRoots[m_stageCurrent]);
255        if(docollide)
256                {
257                m_needcleanup=true;
258                if(!m_deferedcollide)
259                        {
260                        btDbvtTreeCollider      collider(this);
261                        btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
262                        btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
263                        }
264                }       
265        }
266}
267
268//
269void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
270{
271collide(dispatcher);
272#if DBVT_BP_PROFILE
273if(0==(m_pid%DBVT_BP_PROFILING_RATE))
274        {       
275        printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
276        unsigned int    total=m_profiling.m_total;
277        if(total<=0) total=1;
278        printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
279        printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
280        printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
281        printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
282        const unsigned long     sum=m_profiling.m_ddcollide+
283                                                        m_profiling.m_fdcollide+
284                                                        m_profiling.m_cleanup;
285        printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
286        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));
287        clear(m_profiling);
288        m_clock.reset();
289        }
290#endif
291}
292
293//
294void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
295{
296SPC(m_profiling.m_total);
297/* optimize                             */ 
298m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
299if(m_fixedleft)
300        {
301        const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
302        m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
303        m_fixedleft=btMax<int>(0,m_fixedleft-count);
304        }
305/* dynamic -> fixed set */ 
306m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
307btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
308if(current)
309        {
310        btDbvtTreeCollider      collider(this);
311        do      {
312                btDbvtProxy*    next=current->links[1];
313                listremove(current,m_stageRoots[current->stage]);
314                listappend(current,m_stageRoots[STAGECOUNT]);
315                #if DBVT_BP_ACCURATESLEEPING
316                m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
317                collider.proxy=current;
318                btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
319                btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
320                #endif
321                m_sets[0].remove(current->leaf);
322                current->leaf   =       m_sets[1].insert(current->aabb,current);
323                current->stage  =       STAGECOUNT;     
324                current                 =       next;
325                } while(current);
326        m_fixedleft=m_sets[1].m_leaves;
327        m_needcleanup=true;
328        }
329/* collide dynamics             */ 
330        {
331        btDbvtTreeCollider      collider(this);
332        if(m_deferedcollide)
333                {
334                SPC(m_profiling.m_fdcollide);
335                btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
336                }
337        if(m_deferedcollide)
338                {
339                SPC(m_profiling.m_ddcollide);
340                btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
341                }
342        }
343/* clean up                             */ 
344if(m_needcleanup)
345        {
346        SPC(m_profiling.m_cleanup);
347        btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
348        if(pairs.size()>0)
349                {
350                const int       ci=pairs.size();
351                int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
352                for(int i=0;i<ni;++i)
353                        {
354                        btBroadphasePair&       p=pairs[(m_cid+i)%ci];
355                        btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
356                        btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
357                        if(!Intersect(pa->leaf->volume,pb->leaf->volume))
358                                {
359                                #if DBVT_BP_SORTPAIRS
360                                if(pa>pb) btSwap(pa,pb);
361                                #endif
362                                m_paircache->removeOverlappingPair(pa,pb,dispatcher);
363                                --ni;--i;
364                                }
365                        }
366                if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
367                }
368        }
369++m_pid;
370m_newpairs=1;
371m_needcleanup=false;
372if(m_updates_call>0)
373        { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
374        else
375        { m_updates_ratio=0; }
376m_updates_done/=2;
377m_updates_call/=2;
378}
379
380//
381void                                                    btDbvtBroadphase::optimize()
382{
383m_sets[0].optimizeTopDown();
384m_sets[1].optimizeTopDown();
385}
386
387//
388btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
389{
390return(m_paircache);
391}
392
393//
394const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
395{
396return(m_paircache);
397}
398
399//
400void                                                    btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
401{
402
403        ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
404
405if(!m_sets[0].empty())
406        if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
407                                                                        m_sets[1].m_root->volume,bounds);
408                                                        else
409                                                        bounds=m_sets[0].m_root->volume;
410else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
411                                                        else
412                                                        bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
413aabbMin=bounds.Mins();
414aabbMax=bounds.Maxs();
415}
416
417//
418void                                                    btDbvtBroadphase::printStats()
419{}
420
421//
422#if DBVT_BP_ENABLE_BENCHMARK
423
424struct  btBroadphaseBenchmark
425        {
426        struct  Experiment
427                {
428                const char*                     name;
429                int                                     object_count;
430                int                                     update_count;
431                int                                     spawn_count;
432                int                                     iterations;
433                btScalar                        speed;
434                btScalar                        amplitude;
435                };
436        struct  Object
437                {
438                btVector3                       center;
439                btVector3                       extents;
440                btBroadphaseProxy*      proxy;
441                btScalar                        time;
442                void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
443                        {
444                        time            +=      speed;
445                        center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
446                                                        btSin(time)*amplitude/2;
447                        center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
448                                                        btSin(time)*amplitude;
449                        center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
450                        pbi->setAabb(proxy,center-extents,center+extents,0);
451                        }
452                };
453        static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
454        static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
455        static void             OutputTime(const char* name,btClock& c,unsigned count=0)
456                {
457                const unsigned long     us=c.getTimeMicroseconds();
458                const unsigned long     ms=(us+500)/1000;
459                const btScalar          sec=us/(btScalar)(1000*1000);
460                if(count>0)
461                        printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
462                        else
463                        printf("%s : %u us (%u ms)\r\n",name,us,ms);
464                }
465        };
466
467void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
468{
469static const btBroadphaseBenchmark::Experiment          experiments[]=
470        {
471        {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
472        /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
473        {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
474        };
475static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
476btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
477btClock                                                                                                 wallclock;
478/* Begin                        */ 
479for(int iexp=0;iexp<nexperiments;++iexp)
480        {
481        const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
482        const int                                                                       object_count=experiment.object_count;
483        const int                                                                       update_count=(object_count*experiment.update_count)/100;
484        const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
485        const btScalar                                                          speed=experiment.speed; 
486        const btScalar                                                          amplitude=experiment.amplitude;
487        printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
488        printf("\tObjects: %u\r\n",object_count);
489        printf("\tUpdate: %u\r\n",update_count);
490        printf("\tSpawn: %u\r\n",spawn_count);
491        printf("\tSpeed: %f\r\n",speed);
492        printf("\tAmplitude: %f\r\n",amplitude);
493        srand(180673);
494        /* Create objects       */ 
495        wallclock.reset();
496        objects.reserve(object_count);
497        for(int i=0;i<object_count;++i)
498                {
499                btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
500                po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
501                po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
502                po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
503                po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
504                po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
505                po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
506                po->time=btBroadphaseBenchmark::UnitRand()*2000;
507                po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
508                objects.push_back(po);
509                }
510        btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
511        /* First update         */ 
512        wallclock.reset();
513        for(int i=0;i<objects.size();++i)
514                {
515                objects[i]->update(speed,amplitude,pbi);
516                }
517        btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
518        /* Updates                      */ 
519        wallclock.reset();
520        for(int i=0;i<experiment.iterations;++i)
521                {
522                for(int j=0;j<update_count;++j)
523                        {                               
524                        objects[j]->update(speed,amplitude,pbi);
525                        }
526                pbi->calculateOverlappingPairs(0);
527                }
528        btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
529        /* Clean up                     */ 
530        wallclock.reset();
531        for(int i=0;i<objects.size();++i)
532                {
533                pbi->destroyProxy(objects[i]->proxy,0);
534                delete objects[i];
535                }
536        objects.resize(0);
537        btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
538        }
539
540}
541#else
542void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface*)
543{}
544#endif
545
546#if DBVT_BP_PROFILE
547#undef  SPC
548#endif
Note: See TracBrowser for help on using the repository browser.