btOverlappingPairCache.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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         /*if (proxyId1 > proxyId2) 
00145                 btSwap(proxyId1, proxyId2);*/
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 //#include <stdio.h>
00171 
00172 void    btHashedOverlappingPairCache::growTables()
00173 {
00174 
00175         int newCapacity = m_overlappingPairArray.capacity();
00176 
00177         if (m_hashTable.size() < newCapacity)
00178         {
00179                 //grow hashtable and next table
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                         /*if (proxyId1 > proxyId2) 
00204                                 btSwap(proxyId1, proxyId2);*/
00205                         int     hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
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         /*if (proxyId1 > proxyId2) 
00222                 btSwap(proxyId1, proxyId2);*/
00223 
00224         int     hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));      // New hash value with new mask
00225 
00226 
00227         btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
00228         if (pair != NULL)
00229         {
00230                 return pair;
00231         }
00232         /*for(int i=0;i<m_overlappingPairArray.size();++i)
00233                 {
00234                 if(     (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
00235                         (m_overlappingPairArray[i].m_pProxy1==proxy1))
00236                         {
00237                         printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
00238                         internalFindPair(proxy0, proxy1, hash);
00239                         }
00240                 }*/
00241         int count = m_overlappingPairArray.size();
00242         int oldCapacity = m_overlappingPairArray.capacity();
00243         void* mem = &m_overlappingPairArray.expandNonInitializing();
00244 
00245         //this is where we add an actual pair, so also call the 'ghost'
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                 //hash with new capacity
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 //      pair->m_pProxy0 = proxy0;
00260 //      pair->m_pProxy1 = proxy1;
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         /*if (proxyId1 > proxyId2) 
00282                 btSwap(proxyId1, proxyId2);*/
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         // Remove the pair from the hash table.
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         // We now move the last pair into spot of the
00324         // pair being removed. We need to fix the hash
00325         // table indices to support the move.
00326 
00327         int lastPairIndex = m_overlappingPairArray.size() - 1;
00328 
00329         if (m_ghostPairCallback)
00330                 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
00331 
00332         // If the removed pair is the last pair, we are done.
00333         if (lastPairIndex == pairIndex)
00334         {
00335                 m_overlappingPairArray.pop_back();
00336                 return userData;
00337         }
00338 
00339         // Remove the last pair from the hash table.
00340         const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
00341                 /* missing swap here too, Nat. */ 
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         // Copy the last pair into the remove pair's spot.
00365         m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
00366 
00367         // Insert the last pair into the hash table
00368         m_next[pairIndex] = m_hashTable[lastHash];
00369         m_hashTable[lastHash] = pairIndex;
00370 
00371         m_overlappingPairArray.pop_back();
00372 
00373         return userData;
00374 }
00375 //#include <stdio.h>
00376 
00377 void    btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
00378 {
00379 
00380         int i;
00381 
00382 //      printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
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         //don't add overlap with own
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                 //btAssert(it != m_overlappingPairSet.end());
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 //#include <stdio.h>
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         //should already be sorted
00632 }
00633