Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 16.1 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{
55        item->links[0]=0;
56        item->links[1]=list;
57        if(list) list->links[0]=item;
58        list=item;
59}
60
61//
62template <typename T>
63static inline void      listremove(T* item,T*& list)
64{
65        if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
66        if(item->links[1]) item->links[1]->links[0]=item->links[0];
67}
68
69//
70template <typename T>
71static inline int       listcount(T* root)
72{
73        int     n=0;
74        while(root) { ++n;root=root->links[1]; }
75        return(n);
76}
77
78//
79template <typename T>
80static inline void      clear(T& value)
81{
82        static const struct ZeroDummy : T {} zerodummy;
83        value=zerodummy;
84}
85
86//
87// Colliders
88//
89
90/* Tree collider        */ 
91struct  btDbvtTreeCollider : btDbvt::ICollide
92{
93        btDbvtBroadphase*       pbp;
94        btDbvtProxy*            proxy;
95        btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
96        void    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        }
109        void    Process(const btDbvtNode* n)
110        {
111                Process(n,proxy->leaf);
112        }
113};
114
115//
116// btDbvtBroadphase
117//
118
119//
120btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
121{
122        m_deferedcollide        =       false;
123        m_needcleanup           =       true;
124        m_releasepaircache      =       (paircache!=0)?false:true;
125        m_prediction            =       1/(btScalar)2;
126        m_stageCurrent          =       0;
127        m_fixedleft                     =       0;
128        m_fupdates                      =       1;
129        m_dupdates                      =       0;
130        m_cupdates                      =       10;
131        m_newpairs                      =       1;
132        m_updates_call          =       0;
133        m_updates_done          =       0;
134        m_updates_ratio         =       0;
135        m_paircache                     =       paircache?
136paircache       :
137        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//
151btDbvtBroadphase::~btDbvtBroadphase()
152{
153        if(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{
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                btDbvt::collideTV(m_sets[0].m_root,aabb,collider);
186                btDbvt::collideTV(m_sets[1].m_root,aabb,collider);
187        }
188        return(proxy);
189}
190
191//
192void                                                    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
206void    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
213void    btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
214{
215
216        struct  BroadphaseRayTester : btDbvt::ICollide
217        {
218                btBroadphaseRayCallback& m_rayCallback;
219                BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
220                        :m_rayCallback(orgCallback)
221                {
222                }
223                void                                    Process(const btDbvtNode* leaf)
224                {
225                        btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
226                        m_rayCallback.process(proxy);
227                }
228        };     
229
230        BroadphaseRayTester callback(rayCallback);
231
232        m_sets[0].rayTest(      m_sets[0].m_root,
233                rayFrom,
234                rayTo,
235                callback);
236
237        m_sets[1].rayTest(      m_sets[1].m_root,
238                rayFrom,
239                rayTo,
240                callback);
241
242}
243
244//
245void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
246                                                                                                                  const btVector3& aabbMin,
247                                                                                                                  const btVector3& aabbMax,
248                                                                                                                  btDispatcher* /*dispatcher*/)
249{
250        btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
251        ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
252#if DBVT_BP_PREVENTFALSEUPDATE
253        if(NotEqual(aabb,proxy->leaf->volume))
254#endif
255        {
256                bool    docollide=false;
257                if(proxy->stage==STAGECOUNT)
258                {/* fixed -> dynamic set        */ 
259                        m_sets[1].remove(proxy->leaf);
260                        proxy->leaf=m_sets[0].insert(aabb,proxy);
261                        docollide=true;
262                }
263                else
264                {/* dynamic set                         */ 
265                        ++m_updates_call;
266                        if(Intersect(proxy->leaf->volume,aabb))
267                        {/* Moving                              */ 
268
269                                const btVector3 delta=aabbMin-proxy->m_aabbMin;
270                                btVector3               velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
271                                if(delta[0]<0) velocity[0]=-velocity[0];
272                                if(delta[1]<0) velocity[1]=-velocity[1];
273                                if(delta[2]<0) velocity[2]=-velocity[2];
274                                if      (
275#ifdef DBVT_BP_MARGIN                           
276                                        m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
277#else
278                                        m_sets[0].update(proxy->leaf,aabb,velocity)
279#endif
280                                        )
281                                {
282                                        ++m_updates_done;
283                                        docollide=true;
284                                }
285                        }
286                        else
287                        {/* Teleporting                 */ 
288                                m_sets[0].update(proxy->leaf,aabb);
289                                ++m_updates_done;
290                                docollide=true;
291                        }       
292                }
293                listremove(proxy,m_stageRoots[proxy->stage]);
294                proxy->m_aabbMin = aabbMin;
295                proxy->m_aabbMax = aabbMax;
296                proxy->stage    =       m_stageCurrent;
297                listappend(proxy,m_stageRoots[m_stageCurrent]);
298                if(docollide)
299                {
300                        m_needcleanup=true;
301                        if(!m_deferedcollide)
302                        {
303                                btDbvtTreeCollider      collider(this);
304                                btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
305                                btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
306                        }
307                }       
308        }
309}
310
311//
312void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
313{
314        collide(dispatcher);
315#if DBVT_BP_PROFILE
316        if(0==(m_pid%DBVT_BP_PROFILING_RATE))
317        {       
318                printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
319                unsigned int    total=m_profiling.m_total;
320                if(total<=0) total=1;
321                printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
322                printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
323                printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
324                printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
325                const unsigned long     sum=m_profiling.m_ddcollide+
326                        m_profiling.m_fdcollide+
327                        m_profiling.m_cleanup;
328                printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
329                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));
330                clear(m_profiling);
331                m_clock.reset();
332        }
333#endif
334}
335
336//
337void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
338{
339        SPC(m_profiling.m_total);
340        /* optimize                             */ 
341        m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
342        if(m_fixedleft)
343        {
344                const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
345                m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
346                m_fixedleft=btMax<int>(0,m_fixedleft-count);
347        }
348        /* dynamic -> fixed set */ 
349        m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
350        btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
351        if(current)
352        {
353                btDbvtTreeCollider      collider(this);
354                do      {
355                        btDbvtProxy*    next=current->links[1];
356                        listremove(current,m_stageRoots[current->stage]);
357                        listappend(current,m_stageRoots[STAGECOUNT]);
358#if DBVT_BP_ACCURATESLEEPING
359                        m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
360                        collider.proxy=current;
361                        btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
362                        btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
363#endif
364                        m_sets[0].remove(current->leaf);
365                        ATTRIBUTE_ALIGNED16(btDbvtVolume)       curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
366                        current->leaf   =       m_sets[1].insert(curAabb,current);
367                        current->stage  =       STAGECOUNT;     
368                        current                 =       next;
369                } while(current);
370                m_fixedleft=m_sets[1].m_leaves;
371                m_needcleanup=true;
372        }
373        /* collide dynamics             */ 
374        {
375                btDbvtTreeCollider      collider(this);
376                if(m_deferedcollide)
377                {
378                        SPC(m_profiling.m_fdcollide);
379                        btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
380                }
381                if(m_deferedcollide)
382                {
383                        SPC(m_profiling.m_ddcollide);
384                        btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
385                }
386        }
387        /* clean up                             */ 
388        if(m_needcleanup)
389        {
390                SPC(m_profiling.m_cleanup);
391                btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
392                if(pairs.size()>0)
393                {
394                        const int       ci=pairs.size();
395                        int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
396                        for(int i=0;i<ni;++i)
397                        {
398                                btBroadphasePair&       p=pairs[(m_cid+i)%ci];
399                                btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
400                                btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
401                                if(!Intersect(pa->leaf->volume,pb->leaf->volume))
402                                {
403#if DBVT_BP_SORTPAIRS
404                                        if(pa>pb) btSwap(pa,pb);
405#endif
406                                        m_paircache->removeOverlappingPair(pa,pb,dispatcher);
407                                        --ni;--i;
408                                }
409                        }
410                        if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
411                }
412        }
413        ++m_pid;
414        m_newpairs=1;
415        m_needcleanup=false;
416        if(m_updates_call>0)
417        { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
418        else
419        { m_updates_ratio=0; }
420        m_updates_done/=2;
421        m_updates_call/=2;
422}
423
424//
425void                                                    btDbvtBroadphase::optimize()
426{
427        m_sets[0].optimizeTopDown();
428        m_sets[1].optimizeTopDown();
429}
430
431//
432btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
433{
434        return(m_paircache);
435}
436
437//
438const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
439{
440        return(m_paircache);
441}
442
443//
444void                                                    btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
445{
446
447        ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
448
449        if(!m_sets[0].empty())
450                if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
451                        m_sets[1].m_root->volume,bounds);
452                else
453                        bounds=m_sets[0].m_root->volume;
454        else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
455        else
456                bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
457        aabbMin=bounds.Mins();
458        aabbMax=bounds.Maxs();
459}
460
461//
462void                                                    btDbvtBroadphase::printStats()
463{}
464
465//
466#if DBVT_BP_ENABLE_BENCHMARK
467
468struct  btBroadphaseBenchmark
469{
470        struct  Experiment
471        {
472                const char*                     name;
473                int                                     object_count;
474                int                                     update_count;
475                int                                     spawn_count;
476                int                                     iterations;
477                btScalar                        speed;
478                btScalar                        amplitude;
479        };
480        struct  Object
481        {
482                btVector3                       center;
483                btVector3                       extents;
484                btBroadphaseProxy*      proxy;
485                btScalar                        time;
486                void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
487                {
488                        time            +=      speed;
489                        center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
490                                btSin(time)*amplitude/2;
491                        center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
492                                btSin(time)*amplitude;
493                        center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
494                        pbi->setAabb(proxy,center-extents,center+extents,0);
495                }
496        };
497        static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
498        static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
499        static void             OutputTime(const char* name,btClock& c,unsigned count=0)
500        {
501                const unsigned long     us=c.getTimeMicroseconds();
502                const unsigned long     ms=(us+500)/1000;
503                const btScalar          sec=us/(btScalar)(1000*1000);
504                if(count>0)
505                        printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
506                else
507                        printf("%s : %u us (%u ms)\r\n",name,us,ms);
508        }
509};
510
511void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
512{
513        static const btBroadphaseBenchmark::Experiment          experiments[]=
514        {
515                {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
516                /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
517                {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
518        };
519        static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
520        btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
521        btClock                                                                                                 wallclock;
522        /* Begin                        */ 
523        for(int iexp=0;iexp<nexperiments;++iexp)
524        {
525                const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
526                const int                                                                       object_count=experiment.object_count;
527                const int                                                                       update_count=(object_count*experiment.update_count)/100;
528                const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
529                const btScalar                                                          speed=experiment.speed; 
530                const btScalar                                                          amplitude=experiment.amplitude;
531                printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
532                printf("\tObjects: %u\r\n",object_count);
533                printf("\tUpdate: %u\r\n",update_count);
534                printf("\tSpawn: %u\r\n",spawn_count);
535                printf("\tSpeed: %f\r\n",speed);
536                printf("\tAmplitude: %f\r\n",amplitude);
537                srand(180673);
538                /* Create objects       */ 
539                wallclock.reset();
540                objects.reserve(object_count);
541                for(int i=0;i<object_count;++i)
542                {
543                        btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
544                        po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
545                        po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
546                        po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
547                        po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
548                        po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
549                        po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
550                        po->time=btBroadphaseBenchmark::UnitRand()*2000;
551                        po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
552                        objects.push_back(po);
553                }
554                btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
555                /* First update         */ 
556                wallclock.reset();
557                for(int i=0;i<objects.size();++i)
558                {
559                        objects[i]->update(speed,amplitude,pbi);
560                }
561                btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
562                /* Updates                      */ 
563                wallclock.reset();
564                for(int i=0;i<experiment.iterations;++i)
565                {
566                        for(int j=0;j<update_count;++j)
567                        {                               
568                                objects[j]->update(speed,amplitude,pbi);
569                        }
570                        pbi->calculateOverlappingPairs(0);
571                }
572                btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
573                /* Clean up                     */ 
574                wallclock.reset();
575                for(int i=0;i<objects.size();++i)
576                {
577                        pbi->destroyProxy(objects[i]->proxy,0);
578                        delete objects[i];
579                }
580                objects.resize(0);
581                btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
582        }
583
584}
585#else
586void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface*)
587{}
588#endif
589
590#if DBVT_BP_PROFILE
591#undef  SPC
592#endif
Note: See TracBrowser for help on using the repository browser.