00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "btOverlappingPairCache.h"
00019
00020 #include "btDispatcher.h"
00021 #include "btCollisionAlgorithm.h"
00022 #include "LinearMath/btAabbUtil2.h"
00023
00024 #include <stdio.h>
00025
00026 int gOverlappingPairs = 0;
00027
00028 int gRemovePairs =0;
00029 int gAddedPairs =0;
00030 int gFindPairs =0;
00031
00032
00033
00034
00035 btHashedOverlappingPairCache::btHashedOverlappingPairCache():
00036 m_overlapFilterCallback(0),
00037 m_blockedForChanges(false),
00038 m_ghostPairCallback(0)
00039 {
00040 int initialAllocatedSize= 2;
00041 m_overlappingPairArray.reserve(initialAllocatedSize);
00042 growTables();
00043 }
00044
00045
00046
00047
00048 btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
00049 {
00050 }
00051
00052
00053
00054 void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
00055 {
00056 if (pair.m_algorithm)
00057 {
00058 {
00059 pair.m_algorithm->~btCollisionAlgorithm();
00060 dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
00061 pair.m_algorithm=0;
00062 }
00063 }
00064 }
00065
00066
00067
00068
00069 void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
00070 {
00071
00072 class CleanPairCallback : public btOverlapCallback
00073 {
00074 btBroadphaseProxy* m_cleanProxy;
00075 btOverlappingPairCache* m_pairCache;
00076 btDispatcher* m_dispatcher;
00077
00078 public:
00079 CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
00080 :m_cleanProxy(cleanProxy),
00081 m_pairCache(pairCache),
00082 m_dispatcher(dispatcher)
00083 {
00084 }
00085 virtual bool processOverlap(btBroadphasePair& pair)
00086 {
00087 if ((pair.m_pProxy0 == m_cleanProxy) ||
00088 (pair.m_pProxy1 == m_cleanProxy))
00089 {
00090 m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
00091 }
00092 return false;
00093 }
00094
00095 };
00096
00097 CleanPairCallback cleanPairs(proxy,this,dispatcher);
00098
00099 processAllOverlappingPairs(&cleanPairs,dispatcher);
00100
00101 }
00102
00103
00104
00105
00106 void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
00107 {
00108
00109 class RemovePairCallback : public btOverlapCallback
00110 {
00111 btBroadphaseProxy* m_obsoleteProxy;
00112
00113 public:
00114 RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
00115 :m_obsoleteProxy(obsoleteProxy)
00116 {
00117 }
00118 virtual bool processOverlap(btBroadphasePair& pair)
00119 {
00120 return ((pair.m_pProxy0 == m_obsoleteProxy) ||
00121 (pair.m_pProxy1 == m_obsoleteProxy));
00122 }
00123
00124 };
00125
00126
00127 RemovePairCallback removeCallback(proxy);
00128
00129 processAllOverlappingPairs(&removeCallback,dispatcher);
00130 }
00131
00132
00133
00134
00135
00136 btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
00137 {
00138 gFindPairs++;
00139 if(proxy0->m_uniqueId>proxy1->m_uniqueId)
00140 btSwap(proxy0,proxy1);
00141 int proxyId1 = proxy0->getUid();
00142 int proxyId2 = proxy1->getUid();
00143
00144
00145
00146
00147 int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
00148
00149 if (hash >= m_hashTable.size())
00150 {
00151 return NULL;
00152 }
00153
00154 int index = m_hashTable[hash];
00155 while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
00156 {
00157 index = m_next[index];
00158 }
00159
00160 if (index == BT_NULL_PAIR)
00161 {
00162 return NULL;
00163 }
00164
00165 btAssert(index < m_overlappingPairArray.size());
00166
00167 return &m_overlappingPairArray[index];
00168 }
00169
00170
00171
00172 void btHashedOverlappingPairCache::growTables()
00173 {
00174
00175 int newCapacity = m_overlappingPairArray.capacity();
00176
00177 if (m_hashTable.size() < newCapacity)
00178 {
00179
00180 int curHashtableSize = m_hashTable.size();
00181
00182 m_hashTable.resize(newCapacity);
00183 m_next.resize(newCapacity);
00184
00185
00186 int i;
00187
00188 for (i= 0; i < newCapacity; ++i)
00189 {
00190 m_hashTable[i] = BT_NULL_PAIR;
00191 }
00192 for (i = 0; i < newCapacity; ++i)
00193 {
00194 m_next[i] = BT_NULL_PAIR;
00195 }
00196
00197 for(i=0;i<curHashtableSize;i++)
00198 {
00199
00200 const btBroadphasePair& pair = m_overlappingPairArray[i];
00201 int proxyId1 = pair.m_pProxy0->getUid();
00202 int proxyId2 = pair.m_pProxy1->getUid();
00203
00204
00205 int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
00206 m_next[i] = m_hashTable[hashValue];
00207 m_hashTable[hashValue] = i;
00208 }
00209
00210
00211 }
00212 }
00213
00214 btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
00215 {
00216 if(proxy0->m_uniqueId>proxy1->m_uniqueId)
00217 btSwap(proxy0,proxy1);
00218 int proxyId1 = proxy0->getUid();
00219 int proxyId2 = proxy1->getUid();
00220
00221
00222
00223
00224 int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
00225
00226
00227 btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
00228 if (pair != NULL)
00229 {
00230 return pair;
00231 }
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241 int count = m_overlappingPairArray.size();
00242 int oldCapacity = m_overlappingPairArray.capacity();
00243 void* mem = &m_overlappingPairArray.expandNonInitializing();
00244
00245
00246 if (m_ghostPairCallback)
00247 m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
00248
00249 int newCapacity = m_overlappingPairArray.capacity();
00250
00251 if (oldCapacity < newCapacity)
00252 {
00253 growTables();
00254
00255 hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
00256 }
00257
00258 pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
00259
00260
00261 pair->m_algorithm = 0;
00262 pair->m_internalTmpValue = 0;
00263
00264
00265 m_next[count] = m_hashTable[hash];
00266 m_hashTable[hash] = count;
00267
00268 return pair;
00269 }
00270
00271
00272
00273 void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
00274 {
00275 gRemovePairs++;
00276 if(proxy0->m_uniqueId>proxy1->m_uniqueId)
00277 btSwap(proxy0,proxy1);
00278 int proxyId1 = proxy0->getUid();
00279 int proxyId2 = proxy1->getUid();
00280
00281
00282
00283
00284 int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
00285
00286 btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
00287 if (pair == NULL)
00288 {
00289 return 0;
00290 }
00291
00292 cleanOverlappingPair(*pair,dispatcher);
00293
00294 void* userData = pair->m_internalInfo1;
00295
00296 btAssert(pair->m_pProxy0->getUid() == proxyId1);
00297 btAssert(pair->m_pProxy1->getUid() == proxyId2);
00298
00299 int pairIndex = int(pair - &m_overlappingPairArray[0]);
00300 btAssert(pairIndex < m_overlappingPairArray.size());
00301
00302
00303 int index = m_hashTable[hash];
00304 btAssert(index != BT_NULL_PAIR);
00305
00306 int previous = BT_NULL_PAIR;
00307 while (index != pairIndex)
00308 {
00309 previous = index;
00310 index = m_next[index];
00311 }
00312
00313 if (previous != BT_NULL_PAIR)
00314 {
00315 btAssert(m_next[previous] == pairIndex);
00316 m_next[previous] = m_next[pairIndex];
00317 }
00318 else
00319 {
00320 m_hashTable[hash] = m_next[pairIndex];
00321 }
00322
00323
00324
00325
00326
00327 int lastPairIndex = m_overlappingPairArray.size() - 1;
00328
00329 if (m_ghostPairCallback)
00330 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
00331
00332
00333 if (lastPairIndex == pairIndex)
00334 {
00335 m_overlappingPairArray.pop_back();
00336 return userData;
00337 }
00338
00339
00340 const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
00341
00342 int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
00343
00344 index = m_hashTable[lastHash];
00345 btAssert(index != BT_NULL_PAIR);
00346
00347 previous = BT_NULL_PAIR;
00348 while (index != lastPairIndex)
00349 {
00350 previous = index;
00351 index = m_next[index];
00352 }
00353
00354 if (previous != BT_NULL_PAIR)
00355 {
00356 btAssert(m_next[previous] == lastPairIndex);
00357 m_next[previous] = m_next[lastPairIndex];
00358 }
00359 else
00360 {
00361 m_hashTable[lastHash] = m_next[lastPairIndex];
00362 }
00363
00364
00365 m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
00366
00367
00368 m_next[pairIndex] = m_hashTable[lastHash];
00369 m_hashTable[lastHash] = pairIndex;
00370
00371 m_overlappingPairArray.pop_back();
00372
00373 return userData;
00374 }
00375
00376
00377 void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
00378 {
00379
00380 int i;
00381
00382
00383 for (i=0;i<m_overlappingPairArray.size();)
00384 {
00385
00386 btBroadphasePair* pair = &m_overlappingPairArray[i];
00387 if (callback->processOverlap(*pair))
00388 {
00389 removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
00390
00391 gOverlappingPairs--;
00392 } else
00393 {
00394 i++;
00395 }
00396 }
00397 }
00398
00399 void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
00400 {
00402 btBroadphasePairArray tmpPairs;
00403 int i;
00404 for (i=0;i<m_overlappingPairArray.size();i++)
00405 {
00406 tmpPairs.push_back(m_overlappingPairArray[i]);
00407 }
00408
00409 for (i=0;i<tmpPairs.size();i++)
00410 {
00411 removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
00412 }
00413
00414 for (i = 0; i < m_next.size(); i++)
00415 {
00416 m_next[i] = BT_NULL_PAIR;
00417 }
00418
00419 tmpPairs.quickSort(btBroadphasePairSortPredicate());
00420
00421 for (i=0;i<tmpPairs.size();i++)
00422 {
00423 addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
00424 }
00425
00426
00427 }
00428
00429
00430 void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
00431 {
00432 if (!hasDeferredRemoval())
00433 {
00434 btBroadphasePair findPair(*proxy0,*proxy1);
00435
00436 int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
00437 if (findIndex < m_overlappingPairArray.size())
00438 {
00439 gOverlappingPairs--;
00440 btBroadphasePair& pair = m_overlappingPairArray[findIndex];
00441 void* userData = pair.m_internalInfo1;
00442 cleanOverlappingPair(pair,dispatcher);
00443 if (m_ghostPairCallback)
00444 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
00445
00446 m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
00447 m_overlappingPairArray.pop_back();
00448 return userData;
00449 }
00450 }
00451
00452 return 0;
00453 }
00454
00455
00456
00457
00458
00459
00460
00461
00462 btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
00463 {
00464
00465 btAssert(proxy0 != proxy1);
00466
00467 if (!needsBroadphaseCollision(proxy0,proxy1))
00468 return 0;
00469
00470 void* mem = &m_overlappingPairArray.expandNonInitializing();
00471 btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
00472
00473 gOverlappingPairs++;
00474 gAddedPairs++;
00475
00476 if (m_ghostPairCallback)
00477 m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
00478 return pair;
00479
00480 }
00481
00486 btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
00487 {
00488 if (!needsBroadphaseCollision(proxy0,proxy1))
00489 return 0;
00490
00491 btBroadphasePair tmpPair(*proxy0,*proxy1);
00492 int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
00493
00494 if (findIndex < m_overlappingPairArray.size())
00495 {
00496
00497 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
00498 return pair;
00499 }
00500 return 0;
00501 }
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514 void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
00515 {
00516
00517 int i;
00518
00519 for (i=0;i<m_overlappingPairArray.size();)
00520 {
00521
00522 btBroadphasePair* pair = &m_overlappingPairArray[i];
00523 if (callback->processOverlap(*pair))
00524 {
00525 cleanOverlappingPair(*pair,dispatcher);
00526 pair->m_pProxy0 = 0;
00527 pair->m_pProxy1 = 0;
00528 m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
00529 m_overlappingPairArray.pop_back();
00530 gOverlappingPairs--;
00531 } else
00532 {
00533 i++;
00534 }
00535 }
00536 }
00537
00538
00539
00540
00541 btSortedOverlappingPairCache::btSortedOverlappingPairCache():
00542 m_blockedForChanges(false),
00543 m_hasDeferredRemoval(true),
00544 m_overlapFilterCallback(0),
00545 m_ghostPairCallback(0)
00546 {
00547 int initialAllocatedSize= 2;
00548 m_overlappingPairArray.reserve(initialAllocatedSize);
00549 }
00550
00551 btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
00552 {
00553 }
00554
00555 void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
00556 {
00557 if (pair.m_algorithm)
00558 {
00559 {
00560 pair.m_algorithm->~btCollisionAlgorithm();
00561 dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
00562 pair.m_algorithm=0;
00563 gRemovePairs--;
00564 }
00565 }
00566 }
00567
00568
00569 void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
00570 {
00571
00572 class CleanPairCallback : public btOverlapCallback
00573 {
00574 btBroadphaseProxy* m_cleanProxy;
00575 btOverlappingPairCache* m_pairCache;
00576 btDispatcher* m_dispatcher;
00577
00578 public:
00579 CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
00580 :m_cleanProxy(cleanProxy),
00581 m_pairCache(pairCache),
00582 m_dispatcher(dispatcher)
00583 {
00584 }
00585 virtual bool processOverlap(btBroadphasePair& pair)
00586 {
00587 if ((pair.m_pProxy0 == m_cleanProxy) ||
00588 (pair.m_pProxy1 == m_cleanProxy))
00589 {
00590 m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
00591 }
00592 return false;
00593 }
00594
00595 };
00596
00597 CleanPairCallback cleanPairs(proxy,this,dispatcher);
00598
00599 processAllOverlappingPairs(&cleanPairs,dispatcher);
00600
00601 }
00602
00603
00604 void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
00605 {
00606
00607 class RemovePairCallback : public btOverlapCallback
00608 {
00609 btBroadphaseProxy* m_obsoleteProxy;
00610
00611 public:
00612 RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
00613 :m_obsoleteProxy(obsoleteProxy)
00614 {
00615 }
00616 virtual bool processOverlap(btBroadphasePair& pair)
00617 {
00618 return ((pair.m_pProxy0 == m_obsoleteProxy) ||
00619 (pair.m_pProxy1 == m_obsoleteProxy));
00620 }
00621
00622 };
00623
00624 RemovePairCallback removeCallback(proxy);
00625
00626 processAllOverlappingPairs(&removeCallback,dispatcher);
00627 }
00628
00629 void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
00630 {
00631
00632 }
00633