Back to index

supertuxkart  0.5+dfsg1
btMultiSapBroadphase.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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 
00016 #include "btMultiSapBroadphase.h"
00017 
00018 #include "btSimpleBroadphase.h"
00019 #include "LinearMath/btAabbUtil2.h"
00020 #include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
00021 
00023 
00025 extern int gOverlappingPairs;
00026 
00027 /*
00028 class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
00029 {
00030 public:
00031 
00032        virtual btBroadphasePair*   addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
00033        {
00034               return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
00035        }
00036 };
00037 
00038 */
00039 
00040 btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies,btOverlappingPairCache* pairCache)
00041 :m_overlappingPairs(pairCache),
00042 m_ownsPairCache(false),
00043 m_invalidPair(0),
00044 m_optimizedAabbTree(0)
00045 {
00046        if (!m_overlappingPairs)
00047        {
00048               m_ownsPairCache = true;
00049               void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
00050               m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
00051        }
00052 
00053        struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
00054        {
00055               virtual ~btMultiSapOverlapFilterCallback()
00056               {}
00057               // return true when pairs need collision
00058               virtual bool  needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
00059               {
00060                      btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
00061                      btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
00062                      
00063                      bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
00064                      collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
00065        
00066                      return collides;
00067               }
00068        };
00069 
00070        void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
00071        m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
00072 
00073        m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
00074 //     mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
00075 //     m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
00076 }
00077 
00078 btMultiSapBroadphase::~btMultiSapBroadphase()
00079 {
00080        if (m_ownsPairCache)
00081        {
00082               m_overlappingPairs->~btOverlappingPairCache();
00083               btAlignedFree(m_overlappingPairs);
00084        }
00085 }
00086 
00087 
00088 void   btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
00089 {
00090        m_optimizedAabbTree = new btOptimizedBvh();
00091        m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
00092        QuantizedNodeArray&  nodes = m_optimizedAabbTree->getLeafNodeArray();
00093        for (int i=0;i<m_sapBroadphases.size();i++)
00094        {
00095               btQuantizedBvhNode node;
00096               btVector3 aabbMin,aabbMax;
00097               m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
00098               m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
00099               m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
00100               int partId = 0;
00101               node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
00102               nodes.push_back(node);
00103        }
00104        m_optimizedAabbTree->buildInternal();
00105 }
00106 
00107 btBroadphaseProxy*   btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* ignoreMe)
00108 {
00109        //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
00110 
00111        void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
00112        btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
00113        m_multiSapProxies.push_back(proxy);
00114 
00116        setAabb(proxy,aabbMin,aabbMax,dispatcher);
00117        return proxy;
00118 }
00119 
00120 void   btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
00121 {
00123        btAssert(0);
00124 
00125 }
00126 
00127 
00128 void   btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*      childBroadphase)
00129 {
00130        void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
00131        btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy();
00132        bridgeProxyRef->m_childProxy = childProxy;
00133        bridgeProxyRef->m_childBroadphase = childBroadphase;
00134        parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
00135 }
00136 
00137 
00138 bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
00139 {
00140 return
00141 amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
00142 amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
00143 amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
00144 }
00145 
00146 
00147 
00148 
00149 
00150 
00151 //#include <stdio.h>
00152 
00153 void   btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
00154 {
00155        btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
00156        multiProxy->m_aabbMin = aabbMin;
00157        multiProxy->m_aabbMax = aabbMax;
00158        
00159        
00160        bool fullyContained = false;
00161        bool alreadyInSimple = false;
00162        
00163 
00164 
00165        
00166        struct MyNodeOverlapCallback : public btNodeOverlapCallback
00167        {
00168               btMultiSapBroadphase*       m_multiSap;
00169               btMultiSapProxy*            m_multiProxy;
00170               btDispatcher*               m_dispatcher;
00171 
00172               MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
00173                      :m_multiSap(multiSap),
00174                      m_multiProxy(multiProxy),
00175                      m_dispatcher(dispatcher)
00176               {
00177 
00178               }
00179 
00180               virtual void processNode(int nodeSubPart, int broadphaseIndex)
00181               {
00182                      btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
00183 
00184                      int containingBroadphaseIndex = -1;
00185                      //already found?
00186                      for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
00187                      {
00188 
00189                             if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
00190                             {
00191                                    containingBroadphaseIndex = i;
00192                                    break;
00193                             }
00194                      }
00195                      if (containingBroadphaseIndex<0)
00196                      {
00197                             //add it
00198                             btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
00199                             m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
00200 
00201                      }
00202               }
00203        };
00204 
00205        MyNodeOverlapCallback       myNodeCallback(this,multiProxy,dispatcher);
00206 
00207 
00208 
00209        
00210        m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
00211 
00212               for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
00213        {
00214               btVector3 worldAabbMin,worldAabbMax;
00215               multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
00216               bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
00217               if (!overlapsBroadphase)
00218               {
00219                      //remove it now
00220                      btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
00221 
00222                      btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
00223                      bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
00224                      
00225                      multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
00226                      multiProxy->m_bridgeProxies.pop_back();
00227 
00228               }
00229        }
00230 
00231 
00232        /*
00233 
00234        if (1)
00235        {
00236 
00237               //find broadphase that contain this multiProxy
00238               int numChildBroadphases = getBroadphaseArray().size();
00239               for (int i=0;i<numChildBroadphases;i++)
00240               {
00241                      btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
00242                      btVector3 worldAabbMin,worldAabbMax;
00243                      childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
00244                      bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
00245                      
00246               //     fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
00247                      int containingBroadphaseIndex = -1;
00248                      
00249                      //if already contains this
00250                      
00251                      for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
00252                      {
00253                             if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
00254                             {
00255                                    containingBroadphaseIndex = i;
00256                             }
00257                             alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
00258                      }
00259 
00260                      if (overlapsBroadphase)
00261                      {
00262                             if (containingBroadphaseIndex<0)
00263                             {
00264                                    btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
00265                                    childProxy->m_multiSapParentProxy = multiProxy;
00266                                    addToChildBroadphase(multiProxy,childProxy,childBroadphase);
00267                             }
00268                      } else
00269                      {
00270                             if (containingBroadphaseIndex>=0)
00271                             {
00272                                    //remove
00273                                    btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
00274 
00275                                    btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
00276                                    bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
00277                                    
00278                                    multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
00279                                    multiProxy->m_bridgeProxies.pop_back();
00280                             }
00281                      }
00282               }
00283 
00284 
00287               if (0)//!multiProxy->m_bridgeProxies.size())
00288               {
00291                      btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
00292                      childProxy->m_multiSapParentProxy = multiProxy;
00293                      addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
00294               }
00295        }
00296 
00297        if (!multiProxy->m_bridgeProxies.size())
00298        {
00301               btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
00302               childProxy->m_multiSapParentProxy = multiProxy;
00303               addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
00304        }
00305 */
00306 
00307 
00308        //update
00309        for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
00310        {
00311               btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
00312               bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
00313        }
00314 
00315 }
00316 bool stopUpdating=false;
00317 
00318 
00319 
00320 class btMultiSapBroadphasePairSortPredicate
00321 {
00322        public:
00323 
00324               bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
00325               {
00326                             btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
00327                             btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
00328                             btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
00329                             btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
00330 
00331                              return aProxy0 > bProxy0 || 
00332                                    (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
00333                                    (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
00334               }
00335 };
00336 
00337 
00339 void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
00340 {
00341 
00342 //     m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
00343 
00344        if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
00345        {
00346        
00347               btBroadphasePairArray&      overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
00348 
00349        //     quicksort(overlappingPairArray,0,overlappingPairArray.size());
00350 
00351               overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
00352 
00353               //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
00354        //     overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
00355 
00356               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00357               m_invalidPair = 0;
00358 
00359               
00360               int i;
00361 
00362               btBroadphasePair previousPair;
00363               previousPair.m_pProxy0 = 0;
00364               previousPair.m_pProxy1 = 0;
00365               previousPair.m_algorithm = 0;
00366               
00367               
00368               for (i=0;i<overlappingPairArray.size();i++)
00369               {
00370               
00371                      btBroadphasePair& pair = overlappingPairArray[i];
00372 
00373                      btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
00374                      btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
00375                      btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
00376                      btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
00377 
00378                      bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
00379                      
00380                      previousPair = pair;
00381 
00382                      bool needsRemoval = false;
00383 
00384                      if (!isDuplicate)
00385                      {
00386                             bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
00387 
00388                             if (hasOverlap)
00389                             {
00390                                    needsRemoval = false;//callback->processOverlap(pair);
00391                             } else
00392                             {
00393                                    needsRemoval = true;
00394                             }
00395                      } else
00396                      {
00397                             //remove duplicate
00398                             needsRemoval = true;
00399                             //should have no algorithm
00400                             btAssert(!pair.m_algorithm);
00401                      }
00402                      
00403                      if (needsRemoval)
00404                      {
00405                             getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
00406 
00407               //            m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
00408               //            m_overlappingPairArray.pop_back();
00409                             pair.m_pProxy0 = 0;
00410                             pair.m_pProxy1 = 0;
00411                             m_invalidPair++;
00412                             gOverlappingPairs--;
00413                      } 
00414                      
00415               }
00416 
00418        #define CLEAN_INVALID_PAIRS 1
00419        #ifdef CLEAN_INVALID_PAIRS
00420 
00421               //perform a sort, to sort 'invalid' pairs to the end
00422               //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
00423               overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
00424 
00425               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00426               m_invalidPair = 0;
00427        #endif//CLEAN_INVALID_PAIRS
00428               
00429               //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
00430        }
00431 
00432 
00433 }
00434 
00435 
00436 bool   btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
00437 {
00438        btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
00439               btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
00440 
00441               return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
00442                      multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
00443               
00444 }
00445 
00446 
00447 void   btMultiSapBroadphase::printStats()
00448 {
00449 /*     printf("---------------------------------\n");
00450        
00451               printf("btMultiSapBroadphase.h\n");
00452               printf("numHandles = %d\n",m_multiSapProxies.size());
00453                      //find broadphase that contain this multiProxy
00454               int numChildBroadphases = getBroadphaseArray().size();
00455               for (int i=0;i<numChildBroadphases;i++)
00456               {
00457 
00458                      btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
00459                      childBroadphase->printStats();
00460 
00461               }
00462               */
00463 
00464 }