src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btSimpleBroadphase.h"
  17 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
  18 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
  19
  20 #include "LinearMath/btVector3.h"
  21 #include "LinearMath/btTransform.h"
  22 #include "LinearMath/btMatrix3x3.h"
  23 #include "LinearMath/btAabbUtil2.h"
  24
  25 #include <new>
  26
  27 extern int gOverlappingPairs;
  28
  29 void    btSimpleBroadphase::validate()
  30 {
  31         for (int i=0;i<m_numHandles;i++)
  32         {
  33                 for (int j=i+1;j<m_numHandles;j++)
  34                 {
  35                         btAssert(&m_pHandles[i] != &m_pHandles[j]);
  36                 }
  37         }
  38
  39 }
  40
  41 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
  42         :m_pairCache(overlappingPairCache),
  43         m_ownsPairCache(false),
  44         m_invalidPair(0)
  45 {
  46
  47         if (!overlappingPairCache)
  48         {
  49                 void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
  50                 m_pairCache = new (mem)btHashedOverlappingPairCache();
  51                 m_ownsPairCache = true;
  52         }
  53
  54         // allocate handles buffer and put all handles on free list
  55         m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
  56         m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
  57         m_maxHandles = maxProxies;
  58         m_numHandles = 0;
  59         m_firstFreeHandle = 0;
  60         m_LastHandleIndex = -1;
  61
  62
  63         {
  64                 for (int i = m_firstFreeHandle; i < maxProxies; i++)
  65                 {
  66                         m_pHandles[i].SetNextFree(i + 1);
  67                         m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
  68                 }
  69                 m_pHandles[maxProxies - 1].SetNextFree(0);
  70
  71         }
  72
  73 }
  74
  75 btSimpleBroadphase::~btSimpleBroadphase()
  76 {
  77         btAlignedFree(m_pHandlesRawPtr);
  78
  79         if (m_ownsPairCache)
  80         {
  81                 m_pairCache->~btOverlappingPairCache();
  82                 btAlignedFree(m_pairCache);
  83         }
  84 }
  85
  86
  87 btBroadphaseProxy*      btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
  88 {
  89         if (m_numHandles >= m_maxHandles)
  90         {
  91                 btAssert(0);
  92                 return 0; //should never happen, but don't let the game crash ;-)
  93         }
  94         btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
  95
  96         int newHandleIndex = allocHandle();
  97         btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
  98
  99         return proxy;
 100 }
 101
 102 class   RemovingOverlapCallback : public btOverlapCallback
 103 {
 104 protected:
 105         virtual bool    processOverlap(btBroadphasePair& pair)
 106         {
 107                 (void)pair;
 108                 btAssert(0);
 109                 return false;
 110         }
 111 };
 112
 113 class RemovePairContainingProxy
 114 {
 115
 116         btBroadphaseProxy*      m_targetProxy;
 117         public:
 118         virtual ~RemovePairContainingProxy()
 119         {
 120         }
 121 protected:
 122         virtual bool processOverlap(btBroadphasePair& pair)
 123         {
 124                 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
 125                 btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
 126
 127                 return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
 128         };
 129 };
 130
 131 void    btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
 132 {
 133
 134                 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
 135                 freeHandle(proxy0);
 136
 137                 m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
 138
 139                 //validate();
 140
 141 }
 142
 143 void    btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
 144 {
 145         const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 146         aabbMin = sbp->m_aabbMin;
 147         aabbMax = sbp->m_aabbMax;
 148 }
 149
 150 void    btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
 151 {
 152         btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 153         sbp->m_aabbMin = aabbMin;
 154         sbp->m_aabbMax = aabbMax;
 155 }
 156
 157 void    btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
 158 {
 159         for (int i=0; i <= m_LastHandleIndex; i++)
 160         {
 161                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
 162                 if(!proxy->m_clientObject)
 163                 {
 164                         continue;
 165                 }
 166                 rayCallback.process(proxy);
 167         }
 168 }
 169
 170
 171 void    btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
 172 {
 173         for (int i=0; i <= m_LastHandleIndex; i++)
 174         {
 175                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
 176                 if(!proxy->m_clientObject)
 177                 {
 178                         continue;
 179                 }
 180                 if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
 181                 {
 182                         callback.process(proxy);
 183                 }
 184         }
 185 }
 186
 187
 188
 189
 190
 191
 192
 193 bool    btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
 194 {
 195         return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
 196                    proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
 197                    proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
 198
 199 }
 200
 201
 202
 203 //then remove non-overlapping ones
 204 class CheckOverlapCallback : public btOverlapCallback
 205 {
 206 public:
 207         virtual bool processOverlap(btBroadphasePair& pair)
 208         {
 209                 return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
 210         }
 211 };
 212
 213 void    btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 214 {
 215         //first check for new overlapping pairs
 216         int i,j;
 217         if (m_numHandles >= 0)
 218         {
 219                 int new_largest_index = -1;
 220                 for (i=0; i <= m_LastHandleIndex; i++)
 221                 {
 222                         btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
 223                         if(!proxy0->m_clientObject)
 224                         {
 225                                 continue;
 226                         }
 227                         new_largest_index = i;
 228                         for (j=i+1; j <= m_LastHandleIndex; j++)
 229                         {
 230                                 btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
 231                                 btAssert(proxy0 != proxy1);
 232                                 if(!proxy1->m_clientObject)
 233                                 {
 234                                         continue;
 235                                 }
 236
 237                                 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 238                                 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 239
 240                                 if (aabbOverlap(p0,p1))
 241                                 {
 242                                         if ( !m_pairCache->findPair(proxy0,proxy1))
 243                                         {
 244                                                 m_pairCache->addOverlappingPair(proxy0,proxy1);
 245                                         }
 246                                 } else
 247                                 {
 248                                         if (!m_pairCache->hasDeferredRemoval())
 249                                         {
 250                                                 if ( m_pairCache->findPair(proxy0,proxy1))
 251                                                 {
 252                                                         m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
 253                                                 }
 254                                         }
 255                                 }
 256                         }
 257                 }
 258
 259                 m_LastHandleIndex = new_largest_index;
 260
 261                 if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
 262                 {
 263
 264                         btBroadphasePairArray&  overlappingPairArray = m_pairCache->getOverlappingPairArray();
 265
 266                         //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 267                         overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 268
 269                         overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 270                         m_invalidPair = 0;
 271
 272
 273                         btBroadphasePair previousPair;
 274                         previousPair.m_pProxy0 = 0;
 275                         previousPair.m_pProxy1 = 0;
 276                         previousPair.m_algorithm = 0;
 277
 278
 279                         for (i=0;i<overlappingPairArray.size();i++)
 280                         {
 281
 282                                 btBroadphasePair& pair = overlappingPairArray[i];
 283
 284                                 bool isDuplicate = (pair == previousPair);
 285
 286                                 previousPair = pair;
 287
 288                                 bool needsRemoval = false;
 289
 290                                 if (!isDuplicate)
 291                                 {
 292                                         bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
 293
 294                                         if (hasOverlap)
 295                                         {
 296                                                 needsRemoval = false;//callback->processOverlap(pair);
 297                                         } else
 298                                         {
 299                                                 needsRemoval = true;
 300                                         }
 301                                 } else
 302                                 {
 303                                         //remove duplicate
 304                                         needsRemoval = true;
 305                                         //should have no algorithm
 306                                         btAssert(!pair.m_algorithm);
 307                                 }
 308
 309                                 if (needsRemoval)
 310                                 {
 311                                         m_pairCache->cleanOverlappingPair(pair,dispatcher);
 312
 313                                         //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 314                                         //              m_overlappingPairArray.pop_back();
 315                                         pair.m_pProxy0 = 0;
 316                                         pair.m_pProxy1 = 0;
 317                                         m_invalidPair++;
 318                                         gOverlappingPairs--;
 319                                 }
 320
 321                         }
 322
 323                         ///if you don't like to skip the invalid pairs in the array, execute following code:
 324 #define CLEAN_INVALID_PAIRS 1
 325 #ifdef CLEAN_INVALID_PAIRS
 326
 327                         //perform a sort, to sort 'invalid' pairs to the end
 328                         overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 329
 330                         overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 331                         m_invalidPair = 0;
 332 #endif//CLEAN_INVALID_PAIRS
 333
 334                 }
 335         }
 336 }
 337
 338
 339 bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 340 {
 341         btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 342         btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 343         return aabbOverlap(p0,p1);
 344 }
 345
 346 void    btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
 347 {
 348         //not yet
 349 }