| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| |
| |
| #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" |
| |
| #include "BulletCollision/BroadphaseCollision/btDispatcher.h" |
| #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" |
| #include "LinearMath/btAabbUtil2.h" |
| |
| #include <stdio.h> |
| |
| int gOverlappingPairs = 0; |
| |
| int gRemovePairs =0; |
| int gAddedPairs =0; |
| int gFindPairs =0; |
| |
| |
| |
| |
| btHashedOverlappingPairCache::btHashedOverlappingPairCache(): |
| m_overlapFilterCallback(0), |
| m_blockedForChanges(false), |
| m_ghostPairCallback(0) |
| { |
| int initialAllocatedSize= 2; |
| m_overlappingPairArray.reserve(initialAllocatedSize); |
| growTables(); |
| } |
| |
| |
| |
| |
| btHashedOverlappingPairCache::~btHashedOverlappingPairCache() |
| { |
| } |
| |
| |
| |
| void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) |
| { |
| if (pair.m_algorithm) |
| { |
| { |
| pair.m_algorithm->~btCollisionAlgorithm(); |
| dispatcher->freeCollisionAlgorithm(pair.m_algorithm); |
| pair.m_algorithm=0; |
| } |
| } |
| } |
| |
| |
| |
| |
| void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) |
| { |
| |
| class CleanPairCallback : public btOverlapCallback |
| { |
| btBroadphaseProxy* m_cleanProxy; |
| btOverlappingPairCache* m_pairCache; |
| btDispatcher* m_dispatcher; |
| |
| public: |
| CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) |
| :m_cleanProxy(cleanProxy), |
| m_pairCache(pairCache), |
| m_dispatcher(dispatcher) |
| { |
| } |
| virtual bool processOverlap(btBroadphasePair& pair) |
| { |
| if ((pair.m_pProxy0 == m_cleanProxy) || |
| (pair.m_pProxy1 == m_cleanProxy)) |
| { |
| m_pairCache->cleanOverlappingPair(pair,m_dispatcher); |
| } |
| return false; |
| } |
| |
| }; |
| |
| CleanPairCallback cleanPairs(proxy,this,dispatcher); |
| |
| processAllOverlappingPairs(&cleanPairs,dispatcher); |
| |
| } |
| |
| |
| |
| |
| void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) |
| { |
| |
| class RemovePairCallback : public btOverlapCallback |
| { |
| btBroadphaseProxy* m_obsoleteProxy; |
| |
| public: |
| RemovePairCallback(btBroadphaseProxy* obsoleteProxy) |
| :m_obsoleteProxy(obsoleteProxy) |
| { |
| } |
| virtual bool processOverlap(btBroadphasePair& pair) |
| { |
| return ((pair.m_pProxy0 == m_obsoleteProxy) || |
| (pair.m_pProxy1 == m_obsoleteProxy)); |
| } |
| |
| }; |
| |
| |
| RemovePairCallback removeCallback(proxy); |
| |
| processAllOverlappingPairs(&removeCallback,dispatcher); |
| } |
| |
| |
| |
| |
| |
| btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) |
| { |
| gFindPairs++; |
| if(proxy0->m_uniqueId>proxy1->m_uniqueId) |
| btSwap(proxy0,proxy1); |
| int proxyId1 = proxy0->getUid(); |
| int proxyId2 = proxy1->getUid(); |
| |
| /*if (proxyId1 > proxyId2) |
| btSwap(proxyId1, proxyId2);*/ |
| |
| int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); |
| |
| if (hash >= m_hashTable.size()) |
| { |
| return NULL; |
| } |
| |
| int index = m_hashTable[hash]; |
| while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) |
| { |
| index = m_next[index]; |
| } |
| |
| if (index == BT_NULL_PAIR) |
| { |
| return NULL; |
| } |
| |
| btAssert(index < m_overlappingPairArray.size()); |
| |
| return &m_overlappingPairArray[index]; |
| } |
| |
| //#include <stdio.h> |
| |
| void btHashedOverlappingPairCache::growTables() |
| { |
| |
| int newCapacity = m_overlappingPairArray.capacity(); |
| |
| if (m_hashTable.size() < newCapacity) |
| { |
| //grow hashtable and next table |
| int curHashtableSize = m_hashTable.size(); |
| |
| m_hashTable.resize(newCapacity); |
| m_next.resize(newCapacity); |
| |
| |
| int i; |
| |
| for (i= 0; i < newCapacity; ++i) |
| { |
| m_hashTable[i] = BT_NULL_PAIR; |
| } |
| for (i = 0; i < newCapacity; ++i) |
| { |
| m_next[i] = BT_NULL_PAIR; |
| } |
| |
| for(i=0;i<curHashtableSize;i++) |
| { |
| |
| const btBroadphasePair& pair = m_overlappingPairArray[i]; |
| int proxyId1 = pair.m_pProxy0->getUid(); |
| int proxyId2 = pair.m_pProxy1->getUid(); |
| /*if (proxyId1 > proxyId2) |
| btSwap(proxyId1, proxyId2);*/ |
| 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 |
| m_next[i] = m_hashTable[hashValue]; |
| m_hashTable[hashValue] = i; |
| } |
| |
| |
| } |
| } |
| |
| btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) |
| { |
| if(proxy0->m_uniqueId>proxy1->m_uniqueId) |
| btSwap(proxy0,proxy1); |
| int proxyId1 = proxy0->getUid(); |
| int proxyId2 = proxy1->getUid(); |
| |
| /*if (proxyId1 > proxyId2) |
| btSwap(proxyId1, proxyId2);*/ |
| |
| 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 |
| |
| |
| btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); |
| if (pair != NULL) |
| { |
| return pair; |
| } |
| /*for(int i=0;i<m_overlappingPairArray.size();++i) |
| { |
| if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&& |
| (m_overlappingPairArray[i].m_pProxy1==proxy1)) |
| { |
| printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2); |
| internalFindPair(proxy0, proxy1, hash); |
| } |
| }*/ |
| int count = m_overlappingPairArray.size(); |
| int oldCapacity = m_overlappingPairArray.capacity(); |
| void* mem = &m_overlappingPairArray.expand(); |
| |
| //this is where we add an actual pair, so also call the 'ghost' |
| if (m_ghostPairCallback) |
| m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); |
| |
| int newCapacity = m_overlappingPairArray.capacity(); |
| |
| if (oldCapacity < newCapacity) |
| { |
| growTables(); |
| //hash with new capacity |
| hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); |
| } |
| |
| pair = new (mem) btBroadphasePair(*proxy0,*proxy1); |
| // pair->m_pProxy0 = proxy0; |
| // pair->m_pProxy1 = proxy1; |
| pair->m_algorithm = 0; |
| pair->m_internalTmpValue = 0; |
| |
| |
| m_next[count] = m_hashTable[hash]; |
| m_hashTable[hash] = count; |
| |
| return pair; |
| } |
| |
| |
| |
| void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) |
| { |
| gRemovePairs++; |
| if(proxy0->m_uniqueId>proxy1->m_uniqueId) |
| btSwap(proxy0,proxy1); |
| int proxyId1 = proxy0->getUid(); |
| int proxyId2 = proxy1->getUid(); |
| |
| /*if (proxyId1 > proxyId2) |
| btSwap(proxyId1, proxyId2);*/ |
| |
| int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); |
| |
| btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); |
| if (pair == NULL) |
| { |
| return 0; |
| } |
| |
| cleanOverlappingPair(*pair,dispatcher); |
| |
| void* userData = pair->m_internalInfo1; |
| |
| btAssert(pair->m_pProxy0->getUid() == proxyId1); |
| btAssert(pair->m_pProxy1->getUid() == proxyId2); |
| |
| int pairIndex = int(pair - &m_overlappingPairArray[0]); |
| btAssert(pairIndex < m_overlappingPairArray.size()); |
| |
| // Remove the pair from the hash table. |
| int index = m_hashTable[hash]; |
| btAssert(index != BT_NULL_PAIR); |
| |
| int previous = BT_NULL_PAIR; |
| while (index != pairIndex) |
| { |
| previous = index; |
| index = m_next[index]; |
| } |
| |
| if (previous != BT_NULL_PAIR) |
| { |
| btAssert(m_next[previous] == pairIndex); |
| m_next[previous] = m_next[pairIndex]; |
| } |
| else |
| { |
| m_hashTable[hash] = m_next[pairIndex]; |
| } |
| |
| // We now move the last pair into spot of the |
| // pair being removed. We need to fix the hash |
| // table indices to support the move. |
| |
| int lastPairIndex = m_overlappingPairArray.size() - 1; |
| |
| if (m_ghostPairCallback) |
| m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); |
| |
| // If the removed pair is the last pair, we are done. |
| if (lastPairIndex == pairIndex) |
| { |
| m_overlappingPairArray.pop_back(); |
| return userData; |
| } |
| |
| // Remove the last pair from the hash table. |
| const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; |
| /* missing swap here too, Nat. */ |
| 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)); |
| |
| index = m_hashTable[lastHash]; |
| btAssert(index != BT_NULL_PAIR); |
| |
| previous = BT_NULL_PAIR; |
| while (index != lastPairIndex) |
| { |
| previous = index; |
| index = m_next[index]; |
| } |
| |
| if (previous != BT_NULL_PAIR) |
| { |
| btAssert(m_next[previous] == lastPairIndex); |
| m_next[previous] = m_next[lastPairIndex]; |
| } |
| else |
| { |
| m_hashTable[lastHash] = m_next[lastPairIndex]; |
| } |
| |
| // Copy the last pair into the remove pair's spot. |
| m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; |
| |
| // Insert the last pair into the hash table |
| m_next[pairIndex] = m_hashTable[lastHash]; |
| m_hashTable[lastHash] = pairIndex; |
| |
| m_overlappingPairArray.pop_back(); |
| |
| return userData; |
| } |
| //#include <stdio.h> |
| |
| void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) |
| { |
| |
| int i; |
| |
| // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); |
| for (i=0;i<m_overlappingPairArray.size();) |
| { |
| |
| btBroadphasePair* pair = &m_overlappingPairArray[i]; |
| if (callback->processOverlap(*pair)) |
| { |
| removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); |
| |
| gOverlappingPairs--; |
| } else |
| { |
| i++; |
| } |
| } |
| } |
| |
| void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) |
| { |
| ///need to keep hashmap in sync with pair address, so rebuild all |
| btBroadphasePairArray tmpPairs; |
| int i; |
| for (i=0;i<m_overlappingPairArray.size();i++) |
| { |
| tmpPairs.push_back(m_overlappingPairArray[i]); |
| } |
| |
| for (i=0;i<tmpPairs.size();i++) |
| { |
| removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher); |
| } |
| |
| for (i = 0; i < m_next.size(); i++) |
| { |
| m_next[i] = BT_NULL_PAIR; |
| } |
| |
| tmpPairs.quickSort(btBroadphasePairSortPredicate()); |
| |
| for (i=0;i<tmpPairs.size();i++) |
| { |
| addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1); |
| } |
| |
| |
| } |
| |
| |
| void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher ) |
| { |
| if (!hasDeferredRemoval()) |
| { |
| btBroadphasePair findPair(*proxy0,*proxy1); |
| |
| int findIndex = m_overlappingPairArray.findLinearSearch(findPair); |
| if (findIndex < m_overlappingPairArray.size()) |
| { |
| gOverlappingPairs--; |
| btBroadphasePair& pair = m_overlappingPairArray[findIndex]; |
| void* userData = pair.m_internalInfo1; |
| cleanOverlappingPair(pair,dispatcher); |
| if (m_ghostPairCallback) |
| m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); |
| |
| m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); |
| m_overlappingPairArray.pop_back(); |
| return userData; |
| } |
| } |
| |
| return 0; |
| } |
| |
| |
| |
| |
| |
| |
| |
| |
| btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) |
| { |
| //don't add overlap with own |
| btAssert(proxy0 != proxy1); |
| |
| if (!needsBroadphaseCollision(proxy0,proxy1)) |
| return 0; |
| |
| void* mem = &m_overlappingPairArray.expand(); |
| btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); |
| |
| gOverlappingPairs++; |
| gAddedPairs++; |
| |
| if (m_ghostPairCallback) |
| m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); |
| return pair; |
| |
| } |
| |
| ///this findPair becomes really slow. Either sort the list to speedup the query, or |
| ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. |
| ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) |
| ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation |
| btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) |
| { |
| if (!needsBroadphaseCollision(proxy0,proxy1)) |
| return 0; |
| |
| btBroadphasePair tmpPair(*proxy0,*proxy1); |
| int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); |
| |
| if (findIndex < m_overlappingPairArray.size()) |
| { |
| //btAssert(it != m_overlappingPairSet.end()); |
| btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; |
| return pair; |
| } |
| return 0; |
| } |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| //#include <stdio.h> |
| |
| void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) |
| { |
| |
| int i; |
| |
| for (i=0;i<m_overlappingPairArray.size();) |
| { |
| |
| btBroadphasePair* pair = &m_overlappingPairArray[i]; |
| if (callback->processOverlap(*pair)) |
| { |
| cleanOverlappingPair(*pair,dispatcher); |
| pair->m_pProxy0 = 0; |
| pair->m_pProxy1 = 0; |
| m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); |
| m_overlappingPairArray.pop_back(); |
| gOverlappingPairs--; |
| } else |
| { |
| i++; |
| } |
| } |
| } |
| |
| |
| |
| |
| btSortedOverlappingPairCache::btSortedOverlappingPairCache(): |
| m_blockedForChanges(false), |
| m_hasDeferredRemoval(true), |
| m_overlapFilterCallback(0), |
| m_ghostPairCallback(0) |
| { |
| int initialAllocatedSize= 2; |
| m_overlappingPairArray.reserve(initialAllocatedSize); |
| } |
| |
| btSortedOverlappingPairCache::~btSortedOverlappingPairCache() |
| { |
| } |
| |
| void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) |
| { |
| if (pair.m_algorithm) |
| { |
| { |
| pair.m_algorithm->~btCollisionAlgorithm(); |
| dispatcher->freeCollisionAlgorithm(pair.m_algorithm); |
| pair.m_algorithm=0; |
| gRemovePairs--; |
| } |
| } |
| } |
| |
| |
| void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) |
| { |
| |
| class CleanPairCallback : public btOverlapCallback |
| { |
| btBroadphaseProxy* m_cleanProxy; |
| btOverlappingPairCache* m_pairCache; |
| btDispatcher* m_dispatcher; |
| |
| public: |
| CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) |
| :m_cleanProxy(cleanProxy), |
| m_pairCache(pairCache), |
| m_dispatcher(dispatcher) |
| { |
| } |
| virtual bool processOverlap(btBroadphasePair& pair) |
| { |
| if ((pair.m_pProxy0 == m_cleanProxy) || |
| (pair.m_pProxy1 == m_cleanProxy)) |
| { |
| m_pairCache->cleanOverlappingPair(pair,m_dispatcher); |
| } |
| return false; |
| } |
| |
| }; |
| |
| CleanPairCallback cleanPairs(proxy,this,dispatcher); |
| |
| processAllOverlappingPairs(&cleanPairs,dispatcher); |
| |
| } |
| |
| |
| void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) |
| { |
| |
| class RemovePairCallback : public btOverlapCallback |
| { |
| btBroadphaseProxy* m_obsoleteProxy; |
| |
| public: |
| RemovePairCallback(btBroadphaseProxy* obsoleteProxy) |
| :m_obsoleteProxy(obsoleteProxy) |
| { |
| } |
| virtual bool processOverlap(btBroadphasePair& pair) |
| { |
| return ((pair.m_pProxy0 == m_obsoleteProxy) || |
| (pair.m_pProxy1 == m_obsoleteProxy)); |
| } |
| |
| }; |
| |
| RemovePairCallback removeCallback(proxy); |
| |
| processAllOverlappingPairs(&removeCallback,dispatcher); |
| } |
| |
| void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) |
| { |
| //should already be sorted |
| } |
| |