extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.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 "btMultiSapBroadphase.h"
  17
  18 #include "btSimpleBroadphase.h"
  19 #include "LinearMath/btAabbUtil2.h"
  20 #include "btQuantizedBvh.h"
  21
  22 ///     btSapBroadphaseArray    m_sapBroadphases;
  23
  24 ///     btOverlappingPairCache* m_overlappingPairs;
  25 extern int gOverlappingPairs;
  26
  27 /*
  28 class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
  29 {
  30 public:
  31
  32         virtual btBroadphasePair*       addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
  33         {
  34                 return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
  35         }
  36 };
  37
  38 */
  39
  40 btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
  41 :m_overlappingPairs(pairCache),
  42 m_optimizedAabbTree(0),
  43 m_ownsPairCache(false),
  44 m_invalidPair(0)
  45 {
  46         if (!m_overlappingPairs)
  47         {
  48                 m_ownsPairCache = true;
  49                 void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
  50                 m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
  51         }
  52
  53         struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
  54         {
  55                 virtual ~btMultiSapOverlapFilterCallback()
  56                 {}
  57                 // return true when pairs need collision
  58                 virtual bool    needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
  59                 {
  60                         btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
  61                         btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
  62
  63                         bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
  64                         collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
  65
  66                         return collides;
  67                 }
  68         };
  69
  70         void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
  71         m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
  72
  73         m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
  74 //      mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
  75 //      m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
  76 }
  77
  78 btMultiSapBroadphase::~btMultiSapBroadphase()
  79 {
  80         if (m_ownsPairCache)
  81         {
  82                 m_overlappingPairs->~btOverlappingPairCache();
  83                 btAlignedFree(m_overlappingPairs);
  84         }
  85 }
  86
  87
  88 void    btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
  89 {
  90         m_optimizedAabbTree = new btQuantizedBvh();
  91         m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
  92         QuantizedNodeArray&     nodes = m_optimizedAabbTree->getLeafNodeArray();
  93         for (int i=0;i<m_sapBroadphases.size();i++)
  94         {
  95                 btQuantizedBvhNode node;
  96                 btVector3 aabbMin,aabbMax;
  97                 m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
  98                 m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
  99                 m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
 100                 int partId = 0;
 101                 node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
 102                 nodes.push_back(node);
 103         }
 104         m_optimizedAabbTree->buildInternal();
 105 }
 106
 107 btBroadphaseProxy*      btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
 108 {
 109         //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
 110
 111         void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
 112         btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
 113         m_multiSapProxies.push_back(proxy);
 114
 115         ///this should deal with inserting/removal into child broadphases
 116         setAabb(proxy,aabbMin,aabbMax,dispatcher);
 117         return proxy;
 118 }
 119
 120 void    btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
 121 {
 122         ///not yet
 123         btAssert(0);
 124
 125 }
 126
 127
 128 void    btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*  childBroadphase)
 129 {
 130         void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
 131         btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
 132         bridgeProxyRef->m_childProxy = childProxy;
 133         bridgeProxyRef->m_childBroadphase = childBroadphase;
 134         parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
 135 }
 136
 137
 138 bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
 139 bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
 140 {
 141 return
 142 amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
 143 amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
 144 amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
 145 }
 146
 147
 148
 149
 150
 151
 152 void    btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
 153 {
 154         btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
 155         aabbMin = multiProxy->m_aabbMin;
 156         aabbMax = multiProxy->m_aabbMax;
 157 }
 158
 159 void    btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
 160 {
 161         for (int i=0;i<m_multiSapProxies.size();i++)
 162         {
 163                 rayCallback.process(m_multiSapProxies[i]);
 164         }
 165 }
 166
 167
 168 //#include <stdio.h>
 169
 170 void    btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 171 {
 172         btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
 173         multiProxy->m_aabbMin = aabbMin;
 174         multiProxy->m_aabbMax = aabbMax;
 175
 176
 177 //      bool fullyContained = false;
 178 //      bool alreadyInSimple = false;
 179
 180
 181
 182
 183         struct MyNodeOverlapCallback : public btNodeOverlapCallback
 184         {
 185                 btMultiSapBroadphase*   m_multiSap;
 186                 btMultiSapProxy*                m_multiProxy;
 187                 btDispatcher*                   m_dispatcher;
 188
 189                 MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
 190                         :m_multiSap(multiSap),
 191                         m_multiProxy(multiProxy),
 192                         m_dispatcher(dispatcher)
 193                 {
 194
 195                 }
 196
 197                 virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
 198                 {
 199                         btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
 200
 201                         int containingBroadphaseIndex = -1;
 202                         //already found?
 203                         for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
 204                         {
 205
 206                                 if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
 207                                 {
 208                                         containingBroadphaseIndex = i;
 209                                         break;
 210                                 }
 211                         }
 212                         if (containingBroadphaseIndex<0)
 213                         {
 214                                 //add it
 215                                 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);
 216                                 m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
 217
 218                         }
 219                 }
 220         };
 221
 222         MyNodeOverlapCallback   myNodeCallback(this,multiProxy,dispatcher);
 223
 224
 225
 226
 227         if (m_optimizedAabbTree)
 228                 m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
 229
 230         int i;
 231
 232         for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
 233         {
 234                 btVector3 worldAabbMin,worldAabbMax;
 235                 multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
 236                 bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
 237                 if (!overlapsBroadphase)
 238                 {
 239                         //remove it now
 240                         btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
 241
 242                         btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
 243                         bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
 244
 245                         multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
 246                         multiProxy->m_bridgeProxies.pop_back();
 247
 248                 }
 249         }
 250
 251
 252         /*
 253
 254         if (1)
 255         {
 256
 257                 //find broadphase that contain this multiProxy
 258                 int numChildBroadphases = getBroadphaseArray().size();
 259                 for (int i=0;i<numChildBroadphases;i++)
 260                 {
 261                         btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
 262                         btVector3 worldAabbMin,worldAabbMax;
 263                         childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
 264                         bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
 265
 266                 //      fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
 267                         int containingBroadphaseIndex = -1;
 268
 269                         //if already contains this
 270
 271                         for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
 272                         {
 273                                 if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
 274                                 {
 275                                         containingBroadphaseIndex = i;
 276                                 }
 277                                 alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
 278                         }
 279
 280                         if (overlapsBroadphase)
 281                         {
 282                                 if (containingBroadphaseIndex<0)
 283                                 {
 284                                         btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
 285                                         childProxy->m_multiSapParentProxy = multiProxy;
 286                                         addToChildBroadphase(multiProxy,childProxy,childBroadphase);
 287                                 }
 288                         } else
 289                         {
 290                                 if (containingBroadphaseIndex>=0)
 291                                 {
 292                                         //remove
 293                                         btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
 294
 295                                         btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
 296                                         bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
 297
 298                                         multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
 299                                         multiProxy->m_bridgeProxies.pop_back();
 300                                 }
 301                         }
 302                 }
 303
 304
 305                 ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
 306                 ///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
 307                 if (0)//!multiProxy->m_bridgeProxies.size())
 308                 {
 309                         ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
 310                         ///this is needed to be able to calculate the aabb overlap
 311                         btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
 312                         childProxy->m_multiSapParentProxy = multiProxy;
 313                         addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
 314                 }
 315         }
 316
 317         if (!multiProxy->m_bridgeProxies.size())
 318         {
 319                 ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
 320                 ///this is needed to be able to calculate the aabb overlap
 321                 btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
 322                 childProxy->m_multiSapParentProxy = multiProxy;
 323                 addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
 324         }
 325 */
 326
 327
 328         //update
 329         for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
 330         {
 331                 btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
 332                 bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
 333         }
 334
 335 }
 336 bool stopUpdating=false;
 337
 338
 339
 340 class btMultiSapBroadphasePairSortPredicate
 341 {
 342         public:
 343
 344                 bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
 345                 {
 346                                 btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
 347                                 btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
 348                                 btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
 349                                 btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
 350
 351                                  return aProxy0 > bProxy0 ||
 352                                         (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
 353                                         (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm);
 354                 }
 355 };
 356
 357
 358         ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 359 void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 360 {
 361
 362 //      m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
 363
 364         if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
 365         {
 366
 367                 btBroadphasePairArray&  overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
 368
 369         //      quicksort(overlappingPairArray,0,overlappingPairArray.size());
 370
 371                 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
 372
 373                 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 374         //      overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
 375
 376                 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 377                 m_invalidPair = 0;
 378
 379
 380                 int i;
 381
 382                 btBroadphasePair previousPair;
 383                 previousPair.m_pProxy0 = 0;
 384                 previousPair.m_pProxy1 = 0;
 385                 previousPair.m_algorithm = 0;
 386
 387
 388                 for (i=0;i<overlappingPairArray.size();i++)
 389                 {
 390
 391                         btBroadphasePair& pair = overlappingPairArray[i];
 392
 393                         btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
 394                         btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
 395                         btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
 396                         btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
 397
 398                         bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
 399
 400                         previousPair = pair;
 401
 402                         bool needsRemoval = false;
 403
 404                         if (!isDuplicate)
 405                         {
 406                                 bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
 407
 408                                 if (hasOverlap)
 409                                 {
 410                                         needsRemoval = false;//callback->processOverlap(pair);
 411                                 } else
 412                                 {
 413                                         needsRemoval = true;
 414                                 }
 415                         } else
 416                         {
 417                                 //remove duplicate
 418                                 needsRemoval = true;
 419                                 //should have no algorithm
 420                                 btAssert(!pair.m_algorithm);
 421                         }
 422
 423                         if (needsRemoval)
 424                         {
 425                                 getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
 426
 427                 //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 428                 //              m_overlappingPairArray.pop_back();
 429                                 pair.m_pProxy0 = 0;
 430                                 pair.m_pProxy1 = 0;
 431                                 m_invalidPair++;
 432                                 gOverlappingPairs--;
 433                         }
 434
 435                 }
 436
 437         ///if you don't like to skip the invalid pairs in the array, execute following code:
 438         #define CLEAN_INVALID_PAIRS 1
 439         #ifdef CLEAN_INVALID_PAIRS
 440
 441                 //perform a sort, to sort 'invalid' pairs to the end
 442                 //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
 443                 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
 444
 445                 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 446                 m_invalidPair = 0;
 447         #endif//CLEAN_INVALID_PAIRS
 448
 449                 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
 450         }
 451
 452
 453 }
 454
 455
 456 bool    btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
 457 {
 458         btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
 459                 btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
 460
 461                 return  TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
 462                         multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
 463
 464 }
 465
 466
 467 void    btMultiSapBroadphase::printStats()
 468 {
 469 /*      printf("---------------------------------\n");
 470
 471                 printf("btMultiSapBroadphase.h\n");
 472                 printf("numHandles = %d\n",m_multiSapProxies.size());
 473                         //find broadphase that contain this multiProxy
 474                 int numChildBroadphases = getBroadphaseArray().size();
 475                 for (int i=0;i<numChildBroadphases;i++)
 476                 {
 477
 478                         btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
 479                         childBroadphase->printStats();
 480
 481                 }
 482                 */
 483
 484 }
 485
 486 void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
 487 {
 488         // not yet
 489 }