00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 btMultiSapBroadphase::btMultiSapBroadphase(int ,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
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
00075
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* )
00108 {
00109
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* ,btDispatcher* )
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
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
00178
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 , int broadphaseIndex)
00198 {
00199 btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
00200
00201 int containingBroadphaseIndex = -1;
00202
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
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
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
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00307
00308
00311
00312
00313
00314
00315
00316
00317
00318
00321
00322
00323
00324
00325
00326
00327
00328
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 ) const
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
00363
00364 if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
00365 {
00366
00367 btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
00368
00369
00370
00371 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
00372
00373
00374
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;
00411 } else
00412 {
00413 needsRemoval = true;
00414 }
00415 } else
00416 {
00417
00418 needsRemoval = true;
00419
00420 btAssert(!pair.m_algorithm);
00421 }
00422
00423 if (needsRemoval)
00424 {
00425 getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
00426
00427
00428
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
00442
00443 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
00444
00445 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00446 m_invalidPair = 0;
00447 #endif//CLEAN_INVALID_PAIRS
00448
00449
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
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484 }
00485
00486 void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
00487 {
00488
00489 }