btSimpleBroadphase.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 "btSimpleBroadphase.h"
00017 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
00018 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
00019 
00020 #include "LinearMath/btVector3.h"
00021 #include "LinearMath/btTransform.h"
00022 #include "LinearMath/btMatrix3x3.h"
00023 #include "LinearMath/btAabbUtil2.h"
00024 
00025 #include <new>
00026 
00027 extern int gOverlappingPairs;
00028 
00029 void    btSimpleBroadphase::validate()
00030 {
00031         for (int i=0;i<m_numHandles;i++)
00032         {
00033                 for (int j=i+1;j<m_numHandles;j++)
00034                 {
00035                         btAssert(&m_pHandles[i] != &m_pHandles[j]);
00036                 }
00037         }
00038         
00039 }
00040 
00041 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
00042         :m_pairCache(overlappingPairCache),
00043         m_ownsPairCache(false),
00044         m_invalidPair(0)
00045 {
00046 
00047         if (!overlappingPairCache)
00048         {
00049                 void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
00050                 m_pairCache = new (mem)btHashedOverlappingPairCache();
00051                 m_ownsPairCache = true;
00052         }
00053 
00054         // allocate handles buffer and put all handles on free list
00055         m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
00056         m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
00057         m_maxHandles = maxProxies;
00058         m_numHandles = 0;
00059         m_firstFreeHandle = 0;
00060         m_LastHandleIndex = -1;
00061         
00062 
00063         {
00064                 for (int i = m_firstFreeHandle; i < maxProxies; i++)
00065                 {
00066                         m_pHandles[i].SetNextFree(i + 1);
00067                         m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
00068                 }
00069                 m_pHandles[maxProxies - 1].SetNextFree(0);
00070         
00071         }
00072 
00073 }
00074 
00075 btSimpleBroadphase::~btSimpleBroadphase()
00076 {
00077         btAlignedFree(m_pHandlesRawPtr);
00078 
00079         if (m_ownsPairCache)
00080         {
00081                 m_pairCache->~btOverlappingPairCache();
00082                 btAlignedFree(m_pairCache);
00083         }
00084 }
00085 
00086 
00087 btBroadphaseProxy*      btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
00088 {
00089         if (m_numHandles >= m_maxHandles)
00090         {
00091                 btAssert(0);
00092                 return 0; //should never happen, but don't let the game crash ;-)
00093         }
00094         btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
00095 
00096         int newHandleIndex = allocHandle();
00097         btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
00098 
00099         return proxy;
00100 }
00101 
00102 class   RemovingOverlapCallback : public btOverlapCallback
00103 {
00104 protected:
00105         virtual bool    processOverlap(btBroadphasePair& pair)
00106         {
00107                 (void)pair;
00108                 btAssert(0);
00109                 return false;
00110         }
00111 };
00112 
00113 class RemovePairContainingProxy
00114 {
00115 
00116         btBroadphaseProxy*      m_targetProxy;
00117         public:
00118         virtual ~RemovePairContainingProxy()
00119         {
00120         }
00121 protected:
00122         virtual bool processOverlap(btBroadphasePair& pair)
00123         {
00124                 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
00125                 btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
00126 
00127                 return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
00128         };
00129 };
00130 
00131 void    btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
00132 {
00133                 
00134                 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
00135                 freeHandle(proxy0);
00136 
00137                 m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
00138 
00139                 //validate();
00140                 
00141 }
00142 
00143 void    btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
00144 {
00145         const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
00146         aabbMin = sbp->m_aabbMin;
00147         aabbMax = sbp->m_aabbMax;
00148 }
00149 
00150 void    btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
00151 {
00152         btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
00153         sbp->m_aabbMin = aabbMin;
00154         sbp->m_aabbMax = aabbMax;
00155 }
00156 
00157 void    btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
00158 {
00159         for (int i=0; i <= m_LastHandleIndex; i++)
00160         {
00161                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
00162                 if(!proxy->m_clientObject)
00163                 {
00164                         continue;
00165                 }
00166                 rayCallback.process(proxy);
00167         }
00168 }
00169 
00170 
00171 void    btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
00172 {
00173         for (int i=0; i <= m_LastHandleIndex; i++)
00174         {
00175                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
00176                 if(!proxy->m_clientObject)
00177                 {
00178                         continue;
00179                 }
00180                 if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
00181                 {
00182                         callback.process(proxy);
00183                 }
00184         }
00185 }
00186 
00187 
00188 
00189         
00190 
00191 
00192 
00193 bool    btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
00194 {
00195         return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && 
00196                    proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
00197                    proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
00198 
00199 }
00200 
00201 
00202 
00203 //then remove non-overlapping ones
00204 class CheckOverlapCallback : public btOverlapCallback
00205 {
00206 public:
00207         virtual bool processOverlap(btBroadphasePair& pair)
00208         {
00209                 return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
00210         }
00211 };
00212 
00213 void    btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
00214 {
00215         //first check for new overlapping pairs
00216         int i,j;
00217         if (m_numHandles >= 0)
00218         {
00219                 int new_largest_index = -1;
00220                 for (i=0; i <= m_LastHandleIndex; i++)
00221                 {
00222                         btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
00223                         if(!proxy0->m_clientObject)
00224                         {
00225                                 continue;
00226                         }
00227                         new_largest_index = i;
00228                         for (j=i+1; j <= m_LastHandleIndex; j++)
00229                         {
00230                                 btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
00231                                 btAssert(proxy0 != proxy1);
00232                                 if(!proxy1->m_clientObject)
00233                                 {
00234                                         continue;
00235                                 }
00236 
00237                                 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
00238                                 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
00239 
00240                                 if (aabbOverlap(p0,p1))
00241                                 {
00242                                         if ( !m_pairCache->findPair(proxy0,proxy1))
00243                                         {
00244                                                 m_pairCache->addOverlappingPair(proxy0,proxy1);
00245                                         }
00246                                 } else
00247                                 {
00248                                         if (!m_pairCache->hasDeferredRemoval())
00249                                         {
00250                                                 if ( m_pairCache->findPair(proxy0,proxy1))
00251                                                 {
00252                                                         m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
00253                                                 }
00254                                         }
00255                                 }
00256                         }
00257                 }
00258 
00259                 m_LastHandleIndex = new_largest_index;
00260 
00261                 if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
00262                 {
00263 
00264                         btBroadphasePairArray&  overlappingPairArray = m_pairCache->getOverlappingPairArray();
00265 
00266                         //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
00267                         overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00268 
00269                         overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00270                         m_invalidPair = 0;
00271 
00272 
00273                         btBroadphasePair previousPair;
00274                         previousPair.m_pProxy0 = 0;
00275                         previousPair.m_pProxy1 = 0;
00276                         previousPair.m_algorithm = 0;
00277 
00278 
00279                         for (i=0;i<overlappingPairArray.size();i++)
00280                         {
00281 
00282                                 btBroadphasePair& pair = overlappingPairArray[i];
00283 
00284                                 bool isDuplicate = (pair == previousPair);
00285 
00286                                 previousPair = pair;
00287 
00288                                 bool needsRemoval = false;
00289 
00290                                 if (!isDuplicate)
00291                                 {
00292                                         bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
00293 
00294                                         if (hasOverlap)
00295                                         {
00296                                                 needsRemoval = false;//callback->processOverlap(pair);
00297                                         } else
00298                                         {
00299                                                 needsRemoval = true;
00300                                         }
00301                                 } else
00302                                 {
00303                                         //remove duplicate
00304                                         needsRemoval = true;
00305                                         //should have no algorithm
00306                                         btAssert(!pair.m_algorithm);
00307                                 }
00308 
00309                                 if (needsRemoval)
00310                                 {
00311                                         m_pairCache->cleanOverlappingPair(pair,dispatcher);
00312 
00313                                         //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
00314                                         //              m_overlappingPairArray.pop_back();
00315                                         pair.m_pProxy0 = 0;
00316                                         pair.m_pProxy1 = 0;
00317                                         m_invalidPair++;
00318                                         gOverlappingPairs--;
00319                                 } 
00320 
00321                         }
00322 
00324 #define CLEAN_INVALID_PAIRS 1
00325 #ifdef CLEAN_INVALID_PAIRS
00326 
00327                         //perform a sort, to sort 'invalid' pairs to the end
00328                         overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00329 
00330                         overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00331                         m_invalidPair = 0;
00332 #endif//CLEAN_INVALID_PAIRS
00333 
00334                 }
00335         }
00336 }
00337 
00338 
00339 bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
00340 {
00341         btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
00342         btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
00343         return aabbOverlap(p0,p1);
00344 }
00345 
00346 void    btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
00347 {
00348         //not yet
00349 }