Blender V2.61 - r43446

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