2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
16 #include "btOverlappingPairCache.h"
18 #include "btDispatcher.h"
19 #include "btCollisionAlgorithm.h"
20 #include "LinearMath/btAabbUtil2.h"
24 btHashedOverlappingPairCache::btHashedOverlappingPairCache() : m_overlapFilterCallback(0),
25 m_ghostPairCallback(0)
27 int initialAllocatedSize = 2;
28 m_overlappingPairArray.reserve(initialAllocatedSize);
32 btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
36 void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
38 if (pair.m_algorithm && dispatcher)
41 pair.m_algorithm->~btCollisionAlgorithm();
42 dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
48 void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
50 class CleanPairCallback : public btOverlapCallback
52 btBroadphaseProxy* m_cleanProxy;
53 btOverlappingPairCache* m_pairCache;
54 btDispatcher* m_dispatcher;
57 CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
58 : m_cleanProxy(cleanProxy),
59 m_pairCache(pairCache),
60 m_dispatcher(dispatcher)
63 virtual bool processOverlap(btBroadphasePair& pair)
65 if ((pair.m_pProxy0 == m_cleanProxy) ||
66 (pair.m_pProxy1 == m_cleanProxy))
68 m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
74 CleanPairCallback cleanPairs(proxy, this, dispatcher);
76 processAllOverlappingPairs(&cleanPairs, dispatcher);
79 void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
81 class RemovePairCallback : public btOverlapCallback
83 btBroadphaseProxy* m_obsoleteProxy;
86 RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
87 : m_obsoleteProxy(obsoleteProxy)
90 virtual bool processOverlap(btBroadphasePair& pair)
92 return ((pair.m_pProxy0 == m_obsoleteProxy) ||
93 (pair.m_pProxy1 == m_obsoleteProxy));
97 RemovePairCallback removeCallback(proxy);
99 processAllOverlappingPairs(&removeCallback, dispatcher);
102 btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
104 if (proxy0->m_uniqueId > proxy1->m_uniqueId)
105 btSwap(proxy0, proxy1);
106 int proxyId1 = proxy0->getUid();
107 int proxyId2 = proxy1->getUid();
109 /*if (proxyId1 > proxyId2)
110 btSwap(proxyId1, proxyId2);*/
112 int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
114 if (hash >= m_hashTable.size())
119 int index = m_hashTable[hash];
120 while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
122 index = m_next[index];
125 if (index == BT_NULL_PAIR)
130 btAssert(index < m_overlappingPairArray.size());
132 return &m_overlappingPairArray[index];
137 void btHashedOverlappingPairCache::growTables()
139 int newCapacity = m_overlappingPairArray.capacity();
141 if (m_hashTable.size() < newCapacity)
143 //grow hashtable and next table
144 int curHashtableSize = m_hashTable.size();
146 m_hashTable.resize(newCapacity);
147 m_next.resize(newCapacity);
151 for (i = 0; i < newCapacity; ++i)
153 m_hashTable[i] = BT_NULL_PAIR;
155 for (i = 0; i < newCapacity; ++i)
157 m_next[i] = BT_NULL_PAIR;
160 for (i = 0; i < curHashtableSize; i++)
162 const btBroadphasePair& pair = m_overlappingPairArray[i];
163 int proxyId1 = pair.m_pProxy0->getUid();
164 int proxyId2 = pair.m_pProxy1->getUid();
165 /*if (proxyId1 > proxyId2)
166 btSwap(proxyId1, proxyId2);*/
167 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
168 m_next[i] = m_hashTable[hashValue];
169 m_hashTable[hashValue] = i;
174 btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
176 if (proxy0->m_uniqueId > proxy1->m_uniqueId)
177 btSwap(proxy0, proxy1);
178 int proxyId1 = proxy0->getUid();
179 int proxyId2 = proxy1->getUid();
181 /*if (proxyId1 > proxyId2)
182 btSwap(proxyId1, proxyId2);*/
184 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
186 btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
191 /*for(int i=0;i<m_overlappingPairArray.size();++i)
193 if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
194 (m_overlappingPairArray[i].m_pProxy1==proxy1))
196 printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
197 internalFindPair(proxy0, proxy1, hash);
200 int count = m_overlappingPairArray.size();
201 int oldCapacity = m_overlappingPairArray.capacity();
202 void* mem = &m_overlappingPairArray.expandNonInitializing();
204 //this is where we add an actual pair, so also call the 'ghost'
205 if (m_ghostPairCallback)
206 m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
208 int newCapacity = m_overlappingPairArray.capacity();
210 if (oldCapacity < newCapacity)
213 //hash with new capacity
214 hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
217 pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
218 // pair->m_pProxy0 = proxy0;
219 // pair->m_pProxy1 = proxy1;
220 pair->m_algorithm = 0;
221 pair->m_internalTmpValue = 0;
223 m_next[count] = m_hashTable[hash];
224 m_hashTable[hash] = count;
229 void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
231 if (proxy0->m_uniqueId > proxy1->m_uniqueId)
232 btSwap(proxy0, proxy1);
233 int proxyId1 = proxy0->getUid();
234 int proxyId2 = proxy1->getUid();
236 /*if (proxyId1 > proxyId2)
237 btSwap(proxyId1, proxyId2);*/
239 int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
241 btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
247 cleanOverlappingPair(*pair, dispatcher);
249 void* userData = pair->m_internalInfo1;
251 btAssert(pair->m_pProxy0->getUid() == proxyId1);
252 btAssert(pair->m_pProxy1->getUid() == proxyId2);
254 int pairIndex = int(pair - &m_overlappingPairArray[0]);
255 btAssert(pairIndex < m_overlappingPairArray.size());
257 // Remove the pair from the hash table.
258 int index = m_hashTable[hash];
259 btAssert(index != BT_NULL_PAIR);
261 int previous = BT_NULL_PAIR;
262 while (index != pairIndex)
265 index = m_next[index];
268 if (previous != BT_NULL_PAIR)
270 btAssert(m_next[previous] == pairIndex);
271 m_next[previous] = m_next[pairIndex];
275 m_hashTable[hash] = m_next[pairIndex];
278 // We now move the last pair into spot of the
279 // pair being removed. We need to fix the hash
280 // table indices to support the move.
282 int lastPairIndex = m_overlappingPairArray.size() - 1;
284 if (m_ghostPairCallback)
285 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
287 // If the removed pair is the last pair, we are done.
288 if (lastPairIndex == pairIndex)
290 m_overlappingPairArray.pop_back();
294 // Remove the last pair from the hash table.
295 const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
296 /* missing swap here too, Nat. */
297 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));
299 index = m_hashTable[lastHash];
300 btAssert(index != BT_NULL_PAIR);
302 previous = BT_NULL_PAIR;
303 while (index != lastPairIndex)
306 index = m_next[index];
309 if (previous != BT_NULL_PAIR)
311 btAssert(m_next[previous] == lastPairIndex);
312 m_next[previous] = m_next[lastPairIndex];
316 m_hashTable[lastHash] = m_next[lastPairIndex];
319 // Copy the last pair into the remove pair's spot.
320 m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
322 // Insert the last pair into the hash table
323 m_next[pairIndex] = m_hashTable[lastHash];
324 m_hashTable[lastHash] = pairIndex;
326 m_overlappingPairArray.pop_back();
331 #include "LinearMath/btQuickprof.h"
332 void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
334 BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
337 // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
338 for (i = 0; i < m_overlappingPairArray.size();)
340 btBroadphasePair* pair = &m_overlappingPairArray[i];
341 if (callback->processOverlap(*pair))
343 removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
359 class MyPairIndeSortPredicate
362 bool operator()(const MyPairIndex& a, const MyPairIndex& b) const
364 const int uidA0 = a.m_uidA0;
365 const int uidB0 = b.m_uidA0;
366 const int uidA1 = a.m_uidA1;
367 const int uidB1 = b.m_uidA1;
368 return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
372 void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
374 if (dispatchInfo.m_deterministicOverlappingPairs)
376 btBroadphasePairArray& pa = getOverlappingPairArray();
377 btAlignedObjectArray<MyPairIndex> indices;
379 BT_PROFILE("sortOverlappingPairs");
380 indices.resize(pa.size());
381 for (int i = 0; i < indices.size(); i++)
383 const btBroadphasePair& p = pa[i];
384 const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
385 const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
387 indices[i].m_uidA0 = uidA0;
388 indices[i].m_uidA1 = uidA1;
389 indices[i].m_orgIndex = i;
391 indices.quickSort(MyPairIndeSortPredicate());
394 BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
396 for (i = 0; i < indices.size();)
398 btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
399 if (callback->processOverlap(*pair))
401 removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
412 processAllOverlappingPairs(callback, dispatcher);
416 void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
418 ///need to keep hashmap in sync with pair address, so rebuild all
419 btBroadphasePairArray tmpPairs;
421 for (i = 0; i < m_overlappingPairArray.size(); i++)
423 tmpPairs.push_back(m_overlappingPairArray[i]);
426 for (i = 0; i < tmpPairs.size(); i++)
428 removeOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
431 for (i = 0; i < m_next.size(); i++)
433 m_next[i] = BT_NULL_PAIR;
436 tmpPairs.quickSort(btBroadphasePairSortPredicate());
438 for (i = 0; i < tmpPairs.size(); i++)
440 addOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
444 void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
446 if (!hasDeferredRemoval())
448 btBroadphasePair findPair(*proxy0, *proxy1);
450 int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
451 if (findIndex < m_overlappingPairArray.size())
453 btBroadphasePair& pair = m_overlappingPairArray[findIndex];
454 void* userData = pair.m_internalInfo1;
455 cleanOverlappingPair(pair, dispatcher);
456 if (m_ghostPairCallback)
457 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
459 m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
460 m_overlappingPairArray.pop_back();
468 btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
470 //don't add overlap with own
471 btAssert(proxy0 != proxy1);
473 if (!needsBroadphaseCollision(proxy0, proxy1))
476 void* mem = &m_overlappingPairArray.expandNonInitializing();
477 btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
479 if (m_ghostPairCallback)
480 m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
484 ///this findPair becomes really slow. Either sort the list to speedup the query, or
485 ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
486 ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
487 ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
488 btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
490 if (!needsBroadphaseCollision(proxy0, proxy1))
493 btBroadphasePair tmpPair(*proxy0, *proxy1);
494 int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
496 if (findIndex < m_overlappingPairArray.size())
498 //btAssert(it != m_overlappingPairSet.end());
499 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
507 void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
511 for (i = 0; i < m_overlappingPairArray.size();)
513 btBroadphasePair* pair = &m_overlappingPairArray[i];
514 if (callback->processOverlap(*pair))
516 cleanOverlappingPair(*pair, dispatcher);
519 m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
520 m_overlappingPairArray.pop_back();
529 btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false),
530 m_hasDeferredRemoval(true),
531 m_overlapFilterCallback(0),
532 m_ghostPairCallback(0)
534 int initialAllocatedSize = 2;
535 m_overlappingPairArray.reserve(initialAllocatedSize);
538 btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
542 void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
544 if (pair.m_algorithm)
547 pair.m_algorithm->~btCollisionAlgorithm();
548 dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
549 pair.m_algorithm = 0;
554 void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
556 class CleanPairCallback : public btOverlapCallback
558 btBroadphaseProxy* m_cleanProxy;
559 btOverlappingPairCache* m_pairCache;
560 btDispatcher* m_dispatcher;
563 CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
564 : m_cleanProxy(cleanProxy),
565 m_pairCache(pairCache),
566 m_dispatcher(dispatcher)
569 virtual bool processOverlap(btBroadphasePair& pair)
571 if ((pair.m_pProxy0 == m_cleanProxy) ||
572 (pair.m_pProxy1 == m_cleanProxy))
574 m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
580 CleanPairCallback cleanPairs(proxy, this, dispatcher);
582 processAllOverlappingPairs(&cleanPairs, dispatcher);
585 void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
587 class RemovePairCallback : public btOverlapCallback
589 btBroadphaseProxy* m_obsoleteProxy;
592 RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
593 : m_obsoleteProxy(obsoleteProxy)
596 virtual bool processOverlap(btBroadphasePair& pair)
598 return ((pair.m_pProxy0 == m_obsoleteProxy) ||
599 (pair.m_pProxy1 == m_obsoleteProxy));
603 RemovePairCallback removeCallback(proxy);
605 processAllOverlappingPairs(&removeCallback, dispatcher);
608 void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
610 //should already be sorted