Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/netp2/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @ 3008

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

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