btDbvtBroadphase.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00017 
00018 #include "btDbvtBroadphase.h"
00019 
00020 //
00021 // Profiling
00022 //
00023 
00024 #if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
00025 #include <stdio.h>
00026 #endif
00027 
00028 #if DBVT_BP_PROFILE
00029 struct  ProfileScope
00030 {
00031         __forceinline ProfileScope(btClock& clock,unsigned long& value) :
00032         m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
00033         {
00034         }
00035         __forceinline ~ProfileScope()
00036         {
00037                 (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
00038         }
00039         btClock*                m_clock;
00040         unsigned long*  m_value;
00041         unsigned long   m_base;
00042 };
00043 #define SPC(_value_)    ProfileScope    spc_scope(m_clock,_value_)
00044 #else
00045 #define SPC(_value_)
00046 #endif
00047 
00048 //
00049 // Helpers
00050 //
00051 
00052 //
00053 template <typename T>
00054 static inline void      listappend(T* item,T*& list)
00055 {
00056         item->links[0]=0;
00057         item->links[1]=list;
00058         if(list) list->links[0]=item;
00059         list=item;
00060 }
00061 
00062 //
00063 template <typename T>
00064 static inline void      listremove(T* item,T*& list)
00065 {
00066         if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
00067         if(item->links[1]) item->links[1]->links[0]=item->links[0];
00068 }
00069 
00070 //
00071 template <typename T>
00072 static inline int       listcount(T* root)
00073 {
00074         int     n=0;
00075         while(root) { ++n;root=root->links[1]; }
00076         return(n);
00077 }
00078 
00079 //
00080 template <typename T>
00081 static inline void      clear(T& value)
00082 {
00083         static const struct ZeroDummy : T {} zerodummy;
00084         value=zerodummy;
00085 }
00086 
00087 //
00088 // Colliders
00089 //
00090 
00091 /* Tree collider        */ 
00092 struct  btDbvtTreeCollider : btDbvt::ICollide
00093 {
00094         btDbvtBroadphase*       pbp;
00095         btDbvtProxy*            proxy;
00096         btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
00097         void    Process(const btDbvtNode* na,const btDbvtNode* nb)
00098         {
00099                 if(na!=nb)
00100                 {
00101                         btDbvtProxy*    pa=(btDbvtProxy*)na->data;
00102                         btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
00103 #if DBVT_BP_SORTPAIRS
00104                         if(pa->m_uniqueId>pb->m_uniqueId) 
00105                                 btSwap(pa,pb);
00106 #endif
00107                         pbp->m_paircache->addOverlappingPair(pa,pb);
00108                         ++pbp->m_newpairs;
00109                 }
00110         }
00111         void    Process(const btDbvtNode* n)
00112         {
00113                 Process(n,proxy->leaf);
00114         }
00115 };
00116 
00117 //
00118 // btDbvtBroadphase
00119 //
00120 
00121 //
00122 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
00123 {
00124         m_deferedcollide        =       false;
00125         m_needcleanup           =       true;
00126         m_releasepaircache      =       (paircache!=0)?false:true;
00127         m_prediction            =       0;
00128         m_stageCurrent          =       0;
00129         m_fixedleft                     =       0;
00130         m_fupdates                      =       1;
00131         m_dupdates                      =       0;
00132         m_cupdates                      =       10;
00133         m_newpairs                      =       1;
00134         m_updates_call          =       0;
00135         m_updates_done          =       0;
00136         m_updates_ratio         =       0;
00137         m_paircache                     =       paircache? paircache    : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
00138         m_gid                           =       0;
00139         m_pid                           =       0;
00140         m_cid                           =       0;
00141         for(int i=0;i<=STAGECOUNT;++i)
00142         {
00143                 m_stageRoots[i]=0;
00144         }
00145 #if DBVT_BP_PROFILE
00146         clear(m_profiling);
00147 #endif
00148 }
00149 
00150 //
00151 btDbvtBroadphase::~btDbvtBroadphase()
00152 {
00153         if(m_releasepaircache) 
00154         {
00155                 m_paircache->~btOverlappingPairCache();
00156                 btAlignedFree(m_paircache);
00157         }
00158 }
00159 
00160 //
00161 btBroadphaseProxy*                              btDbvtBroadphase::createProxy(  const btVector3& aabbMin,
00162                                                                                                                           const btVector3& aabbMax,
00163                                                                                                                           int /*shapeType*/,
00164                                                                                                                           void* userPtr,
00165                                                                                                                           short int collisionFilterGroup,
00166                                                                                                                           short int collisionFilterMask,
00167                                                                                                                           btDispatcher* /*dispatcher*/,
00168                                                                                                                           void* /*multiSapProxy*/)
00169 {
00170         btDbvtProxy*            proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  aabbMin,aabbMax,userPtr,
00171                 collisionFilterGroup,
00172                 collisionFilterMask);
00173 
00174         btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
00175 
00176         //bproxy->aabb                  =       btDbvtVolume::FromMM(aabbMin,aabbMax);
00177         proxy->stage            =       m_stageCurrent;
00178         proxy->m_uniqueId       =       ++m_gid;
00179         proxy->leaf                     =       m_sets[0].insert(aabb,proxy);
00180         listappend(proxy,m_stageRoots[m_stageCurrent]);
00181         if(!m_deferedcollide)
00182         {
00183                 btDbvtTreeCollider      collider(this);
00184                 collider.proxy=proxy;
00185                 m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
00186                 m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
00187         }
00188         return(proxy);
00189 }
00190 
00191 //
00192 void                                                    btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
00193                                                                                                                            btDispatcher* dispatcher)
00194 {
00195         btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
00196         if(proxy->stage==STAGECOUNT)
00197                 m_sets[1].remove(proxy->leaf);
00198         else
00199                 m_sets[0].remove(proxy->leaf);
00200         listremove(proxy,m_stageRoots[proxy->stage]);
00201         m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
00202         btAlignedFree(proxy);
00203         m_needcleanup=true;
00204 }
00205 
00206 void    btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
00207 {
00208         btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
00209         aabbMin = proxy->m_aabbMin;
00210         aabbMax = proxy->m_aabbMax;
00211 }
00212 
00213 struct  BroadphaseRayTester : btDbvt::ICollide
00214 {
00215         btBroadphaseRayCallback& m_rayCallback;
00216         BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
00217                 :m_rayCallback(orgCallback)
00218         {
00219         }
00220         void                                    Process(const btDbvtNode* leaf)
00221         {
00222                 btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
00223                 m_rayCallback.process(proxy);
00224         }
00225 };      
00226 
00227 void    btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
00228 {
00229         BroadphaseRayTester callback(rayCallback);
00230 
00231         m_sets[0].rayTestInternal(      m_sets[0].m_root,
00232                 rayFrom,
00233                 rayTo,
00234                 rayCallback.m_rayDirectionInverse,
00235                 rayCallback.m_signs,
00236                 rayCallback.m_lambda_max,
00237                 aabbMin,
00238                 aabbMax,
00239                 callback);
00240 
00241         m_sets[1].rayTestInternal(      m_sets[1].m_root,
00242                 rayFrom,
00243                 rayTo,
00244                 rayCallback.m_rayDirectionInverse,
00245                 rayCallback.m_signs,
00246                 rayCallback.m_lambda_max,
00247                 aabbMin,
00248                 aabbMax,
00249                 callback);
00250 
00251 }
00252 
00253 
00254 struct  BroadphaseAabbTester : btDbvt::ICollide
00255 {
00256         btBroadphaseAabbCallback& m_aabbCallback;
00257         BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
00258                 :m_aabbCallback(orgCallback)
00259         {
00260         }
00261         void                                    Process(const btDbvtNode* leaf)
00262         {
00263                 btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
00264                 m_aabbCallback.process(proxy);
00265         }
00266 };      
00267 
00268 void    btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback)
00269 {
00270         BroadphaseAabbTester callback(aabbCallback);
00271 
00272         const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax);
00273                 //process all children, that overlap with  the given AABB bounds
00274         m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
00275         m_sets[1].collideTV(m_sets[1].m_root,bounds,callback);
00276 
00277 }
00278 
00279 
00280 
00281 //
00282 void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
00283                                                                                                                   const btVector3& aabbMin,
00284                                                                                                                   const btVector3& aabbMax,
00285                                                                                                                   btDispatcher* /*dispatcher*/)
00286 {
00287         btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
00288         ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
00289 #if DBVT_BP_PREVENTFALSEUPDATE
00290         if(NotEqual(aabb,proxy->leaf->volume))
00291 #endif
00292         {
00293                 bool    docollide=false;
00294                 if(proxy->stage==STAGECOUNT)
00295                 {/* fixed -> dynamic set        */ 
00296                         m_sets[1].remove(proxy->leaf);
00297                         proxy->leaf=m_sets[0].insert(aabb,proxy);
00298                         docollide=true;
00299                 }
00300                 else
00301                 {/* dynamic set                         */ 
00302                         ++m_updates_call;
00303                         if(Intersect(proxy->leaf->volume,aabb))
00304                         {/* Moving                              */ 
00305 
00306                                 const btVector3 delta=aabbMin-proxy->m_aabbMin;
00307                                 btVector3               velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
00308                                 if(delta[0]<0) velocity[0]=-velocity[0];
00309                                 if(delta[1]<0) velocity[1]=-velocity[1];
00310                                 if(delta[2]<0) velocity[2]=-velocity[2];
00311                                 if      (
00312 #ifdef DBVT_BP_MARGIN                           
00313                                         m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
00314 #else
00315                                         m_sets[0].update(proxy->leaf,aabb,velocity)
00316 #endif
00317                                         )
00318                                 {
00319                                         ++m_updates_done;
00320                                         docollide=true;
00321                                 }
00322                         }
00323                         else
00324                         {/* Teleporting                 */ 
00325                                 m_sets[0].update(proxy->leaf,aabb);
00326                                 ++m_updates_done;
00327                                 docollide=true;
00328                         }       
00329                 }
00330                 listremove(proxy,m_stageRoots[proxy->stage]);
00331                 proxy->m_aabbMin = aabbMin;
00332                 proxy->m_aabbMax = aabbMax;
00333                 proxy->stage    =       m_stageCurrent;
00334                 listappend(proxy,m_stageRoots[m_stageCurrent]);
00335                 if(docollide)
00336                 {
00337                         m_needcleanup=true;
00338                         if(!m_deferedcollide)
00339                         {
00340                                 btDbvtTreeCollider      collider(this);
00341                                 m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
00342                                 m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
00343                         }
00344                 }       
00345         }
00346 }
00347 
00348 
00349 //
00350 void                                                    btDbvtBroadphase::setAabbForceUpdate(           btBroadphaseProxy* absproxy,
00351                                                                                                                   const btVector3& aabbMin,
00352                                                                                                                   const btVector3& aabbMax,
00353                                                                                                                   btDispatcher* /*dispatcher*/)
00354 {
00355         btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
00356         ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
00357         bool    docollide=false;
00358         if(proxy->stage==STAGECOUNT)
00359         {/* fixed -> dynamic set        */ 
00360                 m_sets[1].remove(proxy->leaf);
00361                 proxy->leaf=m_sets[0].insert(aabb,proxy);
00362                 docollide=true;
00363         }
00364         else
00365         {/* dynamic set                         */ 
00366                 ++m_updates_call;
00367                 /* Teleporting                  */ 
00368                 m_sets[0].update(proxy->leaf,aabb);
00369                 ++m_updates_done;
00370                 docollide=true;
00371         }
00372         listremove(proxy,m_stageRoots[proxy->stage]);
00373         proxy->m_aabbMin = aabbMin;
00374         proxy->m_aabbMax = aabbMax;
00375         proxy->stage    =       m_stageCurrent;
00376         listappend(proxy,m_stageRoots[m_stageCurrent]);
00377         if(docollide)
00378         {
00379                 m_needcleanup=true;
00380                 if(!m_deferedcollide)
00381                 {
00382                         btDbvtTreeCollider      collider(this);
00383                         m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
00384                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
00385                 }
00386         }       
00387 }
00388 
00389 //
00390 void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
00391 {
00392         collide(dispatcher);
00393 #if DBVT_BP_PROFILE
00394         if(0==(m_pid%DBVT_BP_PROFILING_RATE))
00395         {       
00396                 printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
00397                 unsigned int    total=m_profiling.m_total;
00398                 if(total<=0) total=1;
00399                 printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
00400                 printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
00401                 printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
00402                 printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
00403                 const unsigned long     sum=m_profiling.m_ddcollide+
00404                         m_profiling.m_fdcollide+
00405                         m_profiling.m_cleanup;
00406                 printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
00407                 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));
00408                 clear(m_profiling);
00409                 m_clock.reset();
00410         }
00411 #endif
00412 
00413         performDeferredRemoval(dispatcher);
00414 
00415 }
00416 
00417 void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
00418 {
00419 
00420         if (m_paircache->hasDeferredRemoval())
00421         {
00422 
00423                 btBroadphasePairArray&  overlappingPairArray = m_paircache->getOverlappingPairArray();
00424 
00425                 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
00426                 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00427 
00428                 int invalidPair = 0;
00429 
00430                 
00431                 int i;
00432 
00433                 btBroadphasePair previousPair;
00434                 previousPair.m_pProxy0 = 0;
00435                 previousPair.m_pProxy1 = 0;
00436                 previousPair.m_algorithm = 0;
00437                 
00438                 
00439                 for (i=0;i<overlappingPairArray.size();i++)
00440                 {
00441                 
00442                         btBroadphasePair& pair = overlappingPairArray[i];
00443 
00444                         bool isDuplicate = (pair == previousPair);
00445 
00446                         previousPair = pair;
00447 
00448                         bool needsRemoval = false;
00449 
00450                         if (!isDuplicate)
00451                         {
00452                                 //important to perform AABB check that is consistent with the broadphase
00453                                 btDbvtProxy*            pa=(btDbvtProxy*)pair.m_pProxy0;
00454                                 btDbvtProxy*            pb=(btDbvtProxy*)pair.m_pProxy1;
00455                                 bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
00456 
00457                                 if (hasOverlap)
00458                                 {
00459                                         needsRemoval = false;
00460                                 } else
00461                                 {
00462                                         needsRemoval = true;
00463                                 }
00464                         } else
00465                         {
00466                                 //remove duplicate
00467                                 needsRemoval = true;
00468                                 //should have no algorithm
00469                                 btAssert(!pair.m_algorithm);
00470                         }
00471                         
00472                         if (needsRemoval)
00473                         {
00474                                 m_paircache->cleanOverlappingPair(pair,dispatcher);
00475 
00476                                 pair.m_pProxy0 = 0;
00477                                 pair.m_pProxy1 = 0;
00478                                 invalidPair++;
00479                         } 
00480                         
00481                 }
00482 
00483                 //perform a sort, to sort 'invalid' pairs to the end
00484                 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00485                 overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
00486         }
00487 }
00488 
00489 //
00490 void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
00491 {
00492         /*printf("---------------------------------------------------------\n");
00493         printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
00494         printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
00495         printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
00496         {
00497                 int i;
00498                 for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
00499                 {
00500                         printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
00501                                 getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
00502                 }
00503                 printf("\n");
00504         }
00505 */
00506 
00507 
00508 
00509         SPC(m_profiling.m_total);
00510         /* optimize                             */ 
00511         m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
00512         if(m_fixedleft)
00513         {
00514                 const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
00515                 m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
00516                 m_fixedleft=btMax<int>(0,m_fixedleft-count);
00517         }
00518         /* dynamic -> fixed set */ 
00519         m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
00520         btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
00521         if(current)
00522         {
00523                 btDbvtTreeCollider      collider(this);
00524                 do      {
00525                         btDbvtProxy*    next=current->links[1];
00526                         listremove(current,m_stageRoots[current->stage]);
00527                         listappend(current,m_stageRoots[STAGECOUNT]);
00528 #if DBVT_BP_ACCURATESLEEPING
00529                         m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
00530                         collider.proxy=current;
00531                         btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
00532                         btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
00533 #endif
00534                         m_sets[0].remove(current->leaf);
00535                         ATTRIBUTE_ALIGNED16(btDbvtVolume)       curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
00536                         current->leaf   =       m_sets[1].insert(curAabb,current);
00537                         current->stage  =       STAGECOUNT;     
00538                         current                 =       next;
00539                 } while(current);
00540                 m_fixedleft=m_sets[1].m_leaves;
00541                 m_needcleanup=true;
00542         }
00543         /* collide dynamics             */ 
00544         {
00545                 btDbvtTreeCollider      collider(this);
00546                 if(m_deferedcollide)
00547                 {
00548                         SPC(m_profiling.m_fdcollide);
00549                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
00550                 }
00551                 if(m_deferedcollide)
00552                 {
00553                         SPC(m_profiling.m_ddcollide);
00554                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
00555                 }
00556         }
00557         /* clean up                             */ 
00558         if(m_needcleanup)
00559         {
00560                 SPC(m_profiling.m_cleanup);
00561                 btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
00562                 if(pairs.size()>0)
00563                 {
00564 
00565                         int                     ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
00566                         for(int i=0;i<ni;++i)
00567                         {
00568                                 btBroadphasePair&       p=pairs[(m_cid+i)%pairs.size()];
00569                                 btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
00570                                 btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
00571                                 if(!Intersect(pa->leaf->volume,pb->leaf->volume))
00572                                 {
00573 #if DBVT_BP_SORTPAIRS
00574                                         if(pa->m_uniqueId>pb->m_uniqueId) 
00575                                                 btSwap(pa,pb);
00576 #endif
00577                                         m_paircache->removeOverlappingPair(pa,pb,dispatcher);
00578                                         --ni;--i;
00579                                 }
00580                         }
00581                         if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
00582                 }
00583         }
00584         ++m_pid;
00585         m_newpairs=1;
00586         m_needcleanup=false;
00587         if(m_updates_call>0)
00588         { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
00589         else
00590         { m_updates_ratio=0; }
00591         m_updates_done/=2;
00592         m_updates_call/=2;
00593 }
00594 
00595 //
00596 void                                                    btDbvtBroadphase::optimize()
00597 {
00598         m_sets[0].optimizeTopDown();
00599         m_sets[1].optimizeTopDown();
00600 }
00601 
00602 //
00603 btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
00604 {
00605         return(m_paircache);
00606 }
00607 
00608 //
00609 const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
00610 {
00611         return(m_paircache);
00612 }
00613 
00614 //
00615 void                                                    btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
00616 {
00617 
00618         ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
00619 
00620         if(!m_sets[0].empty())
00621                 if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
00622                         m_sets[1].m_root->volume,bounds);
00623                 else
00624                         bounds=m_sets[0].m_root->volume;
00625         else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
00626         else
00627                 bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
00628         aabbMin=bounds.Mins();
00629         aabbMax=bounds.Maxs();
00630 }
00631 
00632 void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
00633 {
00634         
00635         int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
00636         if (!totalObjects)
00637         {
00638                 //reset internal dynamic tree data structures
00639                 m_sets[0].clear();
00640                 m_sets[1].clear();
00641                 
00642                 m_deferedcollide        =       false;
00643                 m_needcleanup           =       true;
00644                 m_stageCurrent          =       0;
00645                 m_fixedleft                     =       0;
00646                 m_fupdates                      =       1;
00647                 m_dupdates                      =       0;
00648                 m_cupdates                      =       10;
00649                 m_newpairs                      =       1;
00650                 m_updates_call          =       0;
00651                 m_updates_done          =       0;
00652                 m_updates_ratio         =       0;
00653                 
00654                 m_gid                           =       0;
00655                 m_pid                           =       0;
00656                 m_cid                           =       0;
00657                 for(int i=0;i<=STAGECOUNT;++i)
00658                 {
00659                         m_stageRoots[i]=0;
00660                 }
00661         }
00662 }
00663 
00664 //
00665 void                                                    btDbvtBroadphase::printStats()
00666 {}
00667 
00668 //
00669 #if DBVT_BP_ENABLE_BENCHMARK
00670 
00671 struct  btBroadphaseBenchmark
00672 {
00673         struct  Experiment
00674         {
00675                 const char*                     name;
00676                 int                                     object_count;
00677                 int                                     update_count;
00678                 int                                     spawn_count;
00679                 int                                     iterations;
00680                 btScalar                        speed;
00681                 btScalar                        amplitude;
00682         };
00683         struct  Object
00684         {
00685                 btVector3                       center;
00686                 btVector3                       extents;
00687                 btBroadphaseProxy*      proxy;
00688                 btScalar                        time;
00689                 void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
00690                 {
00691                         time            +=      speed;
00692                         center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
00693                                 btSin(time)*amplitude/2;
00694                         center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
00695                                 btSin(time)*amplitude;
00696                         center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
00697                         pbi->setAabb(proxy,center-extents,center+extents,0);
00698                 }
00699         };
00700         static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
00701         static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
00702         static void             OutputTime(const char* name,btClock& c,unsigned count=0)
00703         {
00704                 const unsigned long     us=c.getTimeMicroseconds();
00705                 const unsigned long     ms=(us+500)/1000;
00706                 const btScalar          sec=us/(btScalar)(1000*1000);
00707                 if(count>0)
00708                         printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
00709                 else
00710                         printf("%s : %u us (%u ms)\r\n",name,us,ms);
00711         }
00712 };
00713 
00714 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
00715 {
00716         static const btBroadphaseBenchmark::Experiment          experiments[]=
00717         {
00718                 {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
00719                 /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
00720                 {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
00721         };
00722         static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
00723         btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
00724         btClock                                                                                                 wallclock;
00725         /* Begin                        */ 
00726         for(int iexp=0;iexp<nexperiments;++iexp)
00727         {
00728                 const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
00729                 const int                                                                       object_count=experiment.object_count;
00730                 const int                                                                       update_count=(object_count*experiment.update_count)/100;
00731                 const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
00732                 const btScalar                                                          speed=experiment.speed; 
00733                 const btScalar                                                          amplitude=experiment.amplitude;
00734                 printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
00735                 printf("\tObjects: %u\r\n",object_count);
00736                 printf("\tUpdate: %u\r\n",update_count);
00737                 printf("\tSpawn: %u\r\n",spawn_count);
00738                 printf("\tSpeed: %f\r\n",speed);
00739                 printf("\tAmplitude: %f\r\n",amplitude);
00740                 srand(180673);
00741                 /* Create objects       */ 
00742                 wallclock.reset();
00743                 objects.reserve(object_count);
00744                 for(int i=0;i<object_count;++i)
00745                 {
00746                         btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
00747                         po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
00748                         po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
00749                         po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
00750                         po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
00751                         po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
00752                         po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
00753                         po->time=btBroadphaseBenchmark::UnitRand()*2000;
00754                         po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
00755                         objects.push_back(po);
00756                 }
00757                 btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
00758                 /* First update         */ 
00759                 wallclock.reset();
00760                 for(int i=0;i<objects.size();++i)
00761                 {
00762                         objects[i]->update(speed,amplitude,pbi);
00763                 }
00764                 btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
00765                 /* Updates                      */ 
00766                 wallclock.reset();
00767                 for(int i=0;i<experiment.iterations;++i)
00768                 {
00769                         for(int j=0;j<update_count;++j)
00770                         {                               
00771                                 objects[j]->update(speed,amplitude,pbi);
00772                         }
00773                         pbi->calculateOverlappingPairs(0);
00774                 }
00775                 btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
00776                 /* Clean up                     */ 
00777                 wallclock.reset();
00778                 for(int i=0;i<objects.size();++i)
00779                 {
00780                         pbi->destroyProxy(objects[i]->proxy,0);
00781                         delete objects[i];
00782                 }
00783                 objects.resize(0);
00784                 btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
00785         }
00786 
00787 }
00788 #else
00789 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface*)
00790 {}
00791 #endif
00792 
00793 #if DBVT_BP_PROFILE
00794 #undef  SPC
00795 #endif
00796