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 "btQuantizedBvh.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_optimizedAabbTree(0),
00043 m_ownsPairCache(false),
00044 m_invalidPair(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 btQuantizedBvh();
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 bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
00140 {
00141 return
00142 amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
00143 amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
00144 amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
00145 }
00146 
00147 
00148 
00149 
00150 
00151 
00152 void    btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
00153 {
00154         btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
00155         aabbMin = multiProxy->m_aabbMin;
00156         aabbMax = multiProxy->m_aabbMax;
00157 }
00158 
00159 void    btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
00160 {
00161         for (int i=0;i<m_multiSapProxies.size();i++)
00162         {
00163                 rayCallback.process(m_multiSapProxies[i]);
00164         }
00165 }
00166 
00167 
00168 //#include <stdio.h>
00169 
00170 void    btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
00171 {
00172         btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
00173         multiProxy->m_aabbMin = aabbMin;
00174         multiProxy->m_aabbMax = aabbMax;
00175         
00176         
00177 //      bool fullyContained = false;
00178 //      bool alreadyInSimple = false;
00179         
00180 
00181 
00182         
00183         struct MyNodeOverlapCallback : public btNodeOverlapCallback
00184         {
00185                 btMultiSapBroadphase*   m_multiSap;
00186                 btMultiSapProxy*                m_multiProxy;
00187                 btDispatcher*                   m_dispatcher;
00188 
00189                 MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
00190                         :m_multiSap(multiSap),
00191                         m_multiProxy(multiProxy),
00192                         m_dispatcher(dispatcher)
00193                 {
00194 
00195                 }
00196 
00197                 virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
00198                 {
00199                         btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
00200 
00201                         int containingBroadphaseIndex = -1;
00202                         //already found?
00203                         for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
00204                         {
00205 
00206                                 if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
00207                                 {
00208                                         containingBroadphaseIndex = i;
00209                                         break;
00210                                 }
00211                         }
00212                         if (containingBroadphaseIndex<0)
00213                         {
00214                                 //add it
00215                                 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);
00216                                 m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
00217 
00218                         }
00219                 }
00220         };
00221 
00222         MyNodeOverlapCallback   myNodeCallback(this,multiProxy,dispatcher);
00223 
00224 
00225 
00226         
00227         if (m_optimizedAabbTree)
00228                 m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
00229 
00230         int i;
00231 
00232         for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
00233         {
00234                 btVector3 worldAabbMin,worldAabbMax;
00235                 multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
00236                 bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
00237                 if (!overlapsBroadphase)
00238                 {
00239                         //remove it now
00240                         btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
00241 
00242                         btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
00243                         bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
00244                         
00245                         multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
00246                         multiProxy->m_bridgeProxies.pop_back();
00247 
00248                 }
00249         }
00250 
00251 
00252         /*
00253 
00254         if (1)
00255         {
00256 
00257                 //find broadphase that contain this multiProxy
00258                 int numChildBroadphases = getBroadphaseArray().size();
00259                 for (int i=0;i<numChildBroadphases;i++)
00260                 {
00261                         btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
00262                         btVector3 worldAabbMin,worldAabbMax;
00263                         childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
00264                         bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
00265                         
00266                 //      fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
00267                         int containingBroadphaseIndex = -1;
00268                         
00269                         //if already contains this
00270                         
00271                         for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
00272                         {
00273                                 if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
00274                                 {
00275                                         containingBroadphaseIndex = i;
00276                                 }
00277                                 alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
00278                         }
00279 
00280                         if (overlapsBroadphase)
00281                         {
00282                                 if (containingBroadphaseIndex<0)
00283                                 {
00284                                         btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
00285                                         childProxy->m_multiSapParentProxy = multiProxy;
00286                                         addToChildBroadphase(multiProxy,childProxy,childBroadphase);
00287                                 }
00288                         } else
00289                         {
00290                                 if (containingBroadphaseIndex>=0)
00291                                 {
00292                                         //remove
00293                                         btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
00294 
00295                                         btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
00296                                         bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
00297                                         
00298                                         multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
00299                                         multiProxy->m_bridgeProxies.pop_back();
00300                                 }
00301                         }
00302                 }
00303 
00304 
00307                 if (0)//!multiProxy->m_bridgeProxies.size())
00308                 {
00311                         btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
00312                         childProxy->m_multiSapParentProxy = multiProxy;
00313                         addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
00314                 }
00315         }
00316 
00317         if (!multiProxy->m_bridgeProxies.size())
00318         {
00321                 btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
00322                 childProxy->m_multiSapParentProxy = multiProxy;
00323                 addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
00324         }
00325 */
00326 
00327 
00328         //update
00329         for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
00330         {
00331                 btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
00332                 bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
00333         }
00334 
00335 }
00336 bool stopUpdating=false;
00337 
00338 
00339 
00340 class btMultiSapBroadphasePairSortPredicate
00341 {
00342         public:
00343 
00344                 bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const
00345                 {
00346                                 btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
00347                                 btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
00348                                 btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
00349                                 btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
00350 
00351                                  return aProxy0 > bProxy0 || 
00352                                         (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
00353                                         (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
00354                 }
00355 };
00356 
00357 
00359 void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
00360 {
00361 
00362 //      m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
00363 
00364         if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
00365         {
00366         
00367                 btBroadphasePairArray&  overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
00368 
00369         //      quicksort(overlappingPairArray,0,overlappingPairArray.size());
00370 
00371                 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
00372 
00373                 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
00374         //      overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
00375 
00376                 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00377                 m_invalidPair = 0;
00378 
00379                 
00380                 int i;
00381 
00382                 btBroadphasePair previousPair;
00383                 previousPair.m_pProxy0 = 0;
00384                 previousPair.m_pProxy1 = 0;
00385                 previousPair.m_algorithm = 0;
00386                 
00387                 
00388                 for (i=0;i<overlappingPairArray.size();i++)
00389                 {
00390                 
00391                         btBroadphasePair& pair = overlappingPairArray[i];
00392 
00393                         btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
00394                         btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
00395                         btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
00396                         btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
00397 
00398                         bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
00399                         
00400                         previousPair = pair;
00401 
00402                         bool needsRemoval = false;
00403 
00404                         if (!isDuplicate)
00405                         {
00406                                 bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
00407 
00408                                 if (hasOverlap)
00409                                 {
00410                                         needsRemoval = false;//callback->processOverlap(pair);
00411                                 } else
00412                                 {
00413                                         needsRemoval = true;
00414                                 }
00415                         } else
00416                         {
00417                                 //remove duplicate
00418                                 needsRemoval = true;
00419                                 //should have no algorithm
00420                                 btAssert(!pair.m_algorithm);
00421                         }
00422                         
00423                         if (needsRemoval)
00424                         {
00425                                 getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
00426 
00427                 //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
00428                 //              m_overlappingPairArray.pop_back();
00429                                 pair.m_pProxy0 = 0;
00430                                 pair.m_pProxy1 = 0;
00431                                 m_invalidPair++;
00432                                 gOverlappingPairs--;
00433                         } 
00434                         
00435                 }
00436 
00438         #define CLEAN_INVALID_PAIRS 1
00439         #ifdef CLEAN_INVALID_PAIRS
00440 
00441                 //perform a sort, to sort 'invalid' pairs to the end
00442                 //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
00443                 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
00444 
00445                 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00446                 m_invalidPair = 0;
00447         #endif//CLEAN_INVALID_PAIRS
00448                 
00449                 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
00450         }
00451 
00452 
00453 }
00454 
00455 
00456 bool    btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
00457 {
00458         btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
00459                 btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
00460 
00461                 return  TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
00462                         multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
00463                 
00464 }
00465 
00466 
00467 void    btMultiSapBroadphase::printStats()
00468 {
00469 /*      printf("---------------------------------\n");
00470         
00471                 printf("btMultiSapBroadphase.h\n");
00472                 printf("numHandles = %d\n",m_multiSapProxies.size());
00473                         //find broadphase that contain this multiProxy
00474                 int numChildBroadphases = getBroadphaseArray().size();
00475                 for (int i=0;i<numChildBroadphases;i++)
00476                 {
00477 
00478                         btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
00479                         childBroadphase->printStats();
00480 
00481                 }
00482                 */
00483 
00484 }
00485 
00486 void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
00487 {
00488         // not yet
00489 }