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