Blender V2.61 - r43446

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 )
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 }