Bullet Collision Detection & Physics Library
btOverlappingPairCache.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
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:
10 
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.
14 */
15 
16 #include "btOverlappingPairCache.h"
17 
18 #include "btDispatcher.h"
19 #include "btCollisionAlgorithm.h"
20 #include "LinearMath/btAabbUtil2.h"
21 
22 #include <stdio.h>
23 
25  m_ghostPairCallback(0)
26 {
27  int initialAllocatedSize = 2;
28  m_overlappingPairArray.reserve(initialAllocatedSize);
29  growTables();
30 }
31 
33 {
34 }
35 
37 {
38  if (pair.m_algorithm && dispatcher)
39  {
40  {
42  dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
43  pair.m_algorithm = 0;
44  }
45  }
46 }
47 
49 {
50  class CleanPairCallback : public btOverlapCallback
51  {
52  btBroadphaseProxy* m_cleanProxy;
53  btOverlappingPairCache* m_pairCache;
54  btDispatcher* m_dispatcher;
55 
56  public:
57  CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
58  : m_cleanProxy(cleanProxy),
59  m_pairCache(pairCache),
60  m_dispatcher(dispatcher)
61  {
62  }
63  virtual bool processOverlap(btBroadphasePair& pair)
64  {
65  if ((pair.m_pProxy0 == m_cleanProxy) ||
66  (pair.m_pProxy1 == m_cleanProxy))
67  {
68  m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
69  }
70  return false;
71  }
72  };
73 
74  CleanPairCallback cleanPairs(proxy, this, dispatcher);
75 
76  processAllOverlappingPairs(&cleanPairs, dispatcher);
77 }
78 
80 {
81  class RemovePairCallback : public btOverlapCallback
82  {
83  btBroadphaseProxy* m_obsoleteProxy;
84 
85  public:
86  RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
87  : m_obsoleteProxy(obsoleteProxy)
88  {
89  }
90  virtual bool processOverlap(btBroadphasePair& pair)
91  {
92  return ((pair.m_pProxy0 == m_obsoleteProxy) ||
93  (pair.m_pProxy1 == m_obsoleteProxy));
94  }
95  };
96 
97  RemovePairCallback removeCallback(proxy);
98 
99  processAllOverlappingPairs(&removeCallback, dispatcher);
100 }
101 
103 {
104  if (proxy0->m_uniqueId > proxy1->m_uniqueId)
105  btSwap(proxy0, proxy1);
106  int proxyId1 = proxy0->getUid();
107  int proxyId2 = proxy1->getUid();
108 
109  /*if (proxyId1 > proxyId2)
110  btSwap(proxyId1, proxyId2);*/
111 
112  int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
113 
114  if (hash >= m_hashTable.size())
115  {
116  return NULL;
117  }
118 
119  int index = m_hashTable[hash];
120  while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
121  {
122  index = m_next[index];
123  }
124 
125  if (index == BT_NULL_PAIR)
126  {
127  return NULL;
128  }
129 
131 
132  return &m_overlappingPairArray[index];
133 }
134 
135 //#include <stdio.h>
136 
138 {
139  int newCapacity = m_overlappingPairArray.capacity();
140 
141  if (m_hashTable.size() < newCapacity)
142  {
143  //grow hashtable and next table
144  int curHashtableSize = m_hashTable.size();
145 
146  m_hashTable.resize(newCapacity);
147  m_next.resize(newCapacity);
148 
149  int i;
150 
151  for (i = 0; i < newCapacity; ++i)
152  {
154  }
155  for (i = 0; i < newCapacity; ++i)
156  {
157  m_next[i] = BT_NULL_PAIR;
158  }
159 
160  for (i = 0; i < curHashtableSize; i++)
161  {
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;
170  }
171  }
172 }
173 
175 {
176  if (proxy0->m_uniqueId > proxy1->m_uniqueId)
177  btSwap(proxy0, proxy1);
178  int proxyId1 = proxy0->getUid();
179  int proxyId2 = proxy1->getUid();
180 
181  /*if (proxyId1 > proxyId2)
182  btSwap(proxyId1, proxyId2);*/
183 
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
185 
186  btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
187  if (pair != NULL)
188  {
189  return pair;
190  }
191  /*for(int i=0;i<m_overlappingPairArray.size();++i)
192  {
193  if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
194  (m_overlappingPairArray[i].m_pProxy1==proxy1))
195  {
196  printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
197  internalFindPair(proxy0, proxy1, hash);
198  }
199  }*/
200  int count = m_overlappingPairArray.size();
201  int oldCapacity = m_overlappingPairArray.capacity();
203 
204  //this is where we add an actual pair, so also call the 'ghost'
206  m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
207 
208  int newCapacity = m_overlappingPairArray.capacity();
209 
210  if (oldCapacity < newCapacity)
211  {
212  growTables();
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));
215  }
216 
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;
222 
223  m_next[count] = m_hashTable[hash];
224  m_hashTable[hash] = count;
225 
226  return pair;
227 }
228 
230 {
231  if (proxy0->m_uniqueId > proxy1->m_uniqueId)
232  btSwap(proxy0, proxy1);
233  int proxyId1 = proxy0->getUid();
234  int proxyId2 = proxy1->getUid();
235 
236  /*if (proxyId1 > proxyId2)
237  btSwap(proxyId1, proxyId2);*/
238 
239  int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
240 
241  btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
242  if (pair == NULL)
243  {
244  return 0;
245  }
246 
247  cleanOverlappingPair(*pair, dispatcher);
248 
249  void* userData = pair->m_internalInfo1;
250 
251  btAssert(pair->m_pProxy0->getUid() == proxyId1);
252  btAssert(pair->m_pProxy1->getUid() == proxyId2);
253 
254  int pairIndex = int(pair - &m_overlappingPairArray[0]);
255  btAssert(pairIndex < m_overlappingPairArray.size());
256 
257  // Remove the pair from the hash table.
258  int index = m_hashTable[hash];
259  btAssert(index != BT_NULL_PAIR);
260 
261  int previous = BT_NULL_PAIR;
262  while (index != pairIndex)
263  {
264  previous = index;
265  index = m_next[index];
266  }
267 
268  if (previous != BT_NULL_PAIR)
269  {
270  btAssert(m_next[previous] == pairIndex);
271  m_next[previous] = m_next[pairIndex];
272  }
273  else
274  {
275  m_hashTable[hash] = m_next[pairIndex];
276  }
277 
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.
281 
282  int lastPairIndex = m_overlappingPairArray.size() - 1;
283 
285  m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
286 
287  // If the removed pair is the last pair, we are done.
288  if (lastPairIndex == pairIndex)
289  {
291  return userData;
292  }
293 
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));
298 
299  index = m_hashTable[lastHash];
300  btAssert(index != BT_NULL_PAIR);
301 
302  previous = BT_NULL_PAIR;
303  while (index != lastPairIndex)
304  {
305  previous = index;
306  index = m_next[index];
307  }
308 
309  if (previous != BT_NULL_PAIR)
310  {
311  btAssert(m_next[previous] == lastPairIndex);
312  m_next[previous] = m_next[lastPairIndex];
313  }
314  else
315  {
316  m_hashTable[lastHash] = m_next[lastPairIndex];
317  }
318 
319  // Copy the last pair into the remove pair's spot.
320  m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
321 
322  // Insert the last pair into the hash table
323  m_next[pairIndex] = m_hashTable[lastHash];
324  m_hashTable[lastHash] = pairIndex;
325 
327 
328  return userData;
329 }
330 //#include <stdio.h>
331 #include "LinearMath/btQuickprof.h"
333 {
334  BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
335  int i;
336 
337  // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
338  for (i = 0; i < m_overlappingPairArray.size();)
339  {
341  if (callback->processOverlap(*pair))
342  {
343  removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
344  }
345  else
346  {
347  i++;
348  }
349  }
350 }
351 
353 {
355  int m_uidA0;
356  int m_uidA1;
357 };
358 
360 {
361 public:
362  bool operator()(const MyPairIndex& a, const MyPairIndex& b) const
363  {
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);
369  }
370 };
371 
373 {
374  if (dispatchInfo.m_deterministicOverlappingPairs)
375  {
378  {
379  BT_PROFILE("sortOverlappingPairs");
380  indices.resize(pa.size());
381  for (int i = 0; i < indices.size(); i++)
382  {
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;
386 
387  indices[i].m_uidA0 = uidA0;
388  indices[i].m_uidA1 = uidA1;
389  indices[i].m_orgIndex = i;
390  }
392  }
393  {
394  BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
395  int i;
396  for (i = 0; i < indices.size();)
397  {
398  btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
399  if (callback->processOverlap(*pair))
400  {
401  removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
402  }
403  else
404  {
405  i++;
406  }
407  }
408  }
409  }
410  else
411  {
412  processAllOverlappingPairs(callback, dispatcher);
413  }
414 }
415 
417 {
419  btBroadphasePairArray tmpPairs;
420  int i;
421  for (i = 0; i < m_overlappingPairArray.size(); i++)
422  {
423  tmpPairs.push_back(m_overlappingPairArray[i]);
424  }
425 
426  for (i = 0; i < tmpPairs.size(); i++)
427  {
428  removeOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
429  }
430 
431  for (i = 0; i < m_next.size(); i++)
432  {
433  m_next[i] = BT_NULL_PAIR;
434  }
435 
437 
438  for (i = 0; i < tmpPairs.size(); i++)
439  {
440  addOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
441  }
442 }
443 
445 {
446  if (!hasDeferredRemoval())
447  {
448  btBroadphasePair findPair(*proxy0, *proxy1);
449 
451  if (findIndex < m_overlappingPairArray.size())
452  {
453  btBroadphasePair& pair = m_overlappingPairArray[findIndex];
454  void* userData = pair.m_internalInfo1;
455  cleanOverlappingPair(pair, dispatcher);
457  m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
458 
461  return userData;
462  }
463  }
464 
465  return 0;
466 }
467 
469 {
470  //don't add overlap with own
471  btAssert(proxy0 != proxy1);
472 
473  if (!needsBroadphaseCollision(proxy0, proxy1))
474  return 0;
475 
477  btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
478 
480  m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
481  return pair;
482 }
483 
489 {
490  if (!needsBroadphaseCollision(proxy0, proxy1))
491  return 0;
492 
493  btBroadphasePair tmpPair(*proxy0, *proxy1);
494  int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
495 
496  if (findIndex < m_overlappingPairArray.size())
497  {
498  //btAssert(it != m_overlappingPairSet.end());
499  btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
500  return pair;
501  }
502  return 0;
503 }
504 
505 //#include <stdio.h>
506 
508 {
509  int i;
510 
511  for (i = 0; i < m_overlappingPairArray.size();)
512  {
514  if (callback->processOverlap(*pair))
515  {
516  cleanOverlappingPair(*pair, dispatcher);
517  pair->m_pProxy0 = 0;
518  pair->m_pProxy1 = 0;
521  }
522  else
523  {
524  i++;
525  }
526  }
527 }
528 
530  m_hasDeferredRemoval(true),
531  m_overlapFilterCallback(0),
532  m_ghostPairCallback(0)
533 {
534  int initialAllocatedSize = 2;
535  m_overlappingPairArray.reserve(initialAllocatedSize);
536 }
537 
539 {
540 }
541 
543 {
544  if (pair.m_algorithm)
545  {
546  {
548  dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
549  pair.m_algorithm = 0;
550  }
551  }
552 }
553 
555 {
556  class CleanPairCallback : public btOverlapCallback
557  {
558  btBroadphaseProxy* m_cleanProxy;
559  btOverlappingPairCache* m_pairCache;
560  btDispatcher* m_dispatcher;
561 
562  public:
563  CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
564  : m_cleanProxy(cleanProxy),
565  m_pairCache(pairCache),
566  m_dispatcher(dispatcher)
567  {
568  }
569  virtual bool processOverlap(btBroadphasePair& pair)
570  {
571  if ((pair.m_pProxy0 == m_cleanProxy) ||
572  (pair.m_pProxy1 == m_cleanProxy))
573  {
574  m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
575  }
576  return false;
577  }
578  };
579 
580  CleanPairCallback cleanPairs(proxy, this, dispatcher);
581 
582  processAllOverlappingPairs(&cleanPairs, dispatcher);
583 }
584 
586 {
587  class RemovePairCallback : public btOverlapCallback
588  {
589  btBroadphaseProxy* m_obsoleteProxy;
590 
591  public:
592  RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
593  : m_obsoleteProxy(obsoleteProxy)
594  {
595  }
596  virtual bool processOverlap(btBroadphasePair& pair)
597  {
598  return ((pair.m_pProxy0 == m_obsoleteProxy) ||
599  (pair.m_pProxy1 == m_obsoleteProxy));
600  }
601  };
602 
603  RemovePairCallback removeCallback(proxy);
604 
605  processAllOverlappingPairs(&removeCallback, dispatcher);
606 }
607 
609 {
610  //should already be sorted
611 }
const int BT_NULL_PAIR
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
void btSwap(T &a, T &b)
Definition: btScalar.h:643
#define btAssert(x)
Definition: btScalar.h:153
bool operator()(const MyPairIndex &a, const MyPairIndex &b) const
int size() const
return the number of elements in the array
int findLinearSearch(const T &key) const
void resize(int newsize, const T &fillData=T())
void swap(int index0, int index1)
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
virtual void freeCollisionAlgorithm(void *ptr)=0
void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)
void cleanProxyFromPairs(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
btAlignedObjectArray< int > m_next
btBroadphasePairArray m_overlappingPairArray
btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
virtual void sortOverlappingPairs(btDispatcher *dispatcher)
btBroadphasePair * internalAddPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
bool equalsPair(const btBroadphasePair &pair, int proxyId1, int proxyId2)
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
btBroadphasePairArray & getOverlappingPairArray()
unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
btAlignedObjectArray< int > m_hashTable
btOverlappingPairCallback * m_ghostPairCallback
btBroadphasePair * internalFindPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, int hash)
virtual void processAllOverlappingPairs(btOverlapCallback *, btDispatcher *dispatcher)
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
virtual void processAllOverlappingPairs(btOverlapCallback *, btDispatcher *dispatcher)
btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
this findPair becomes really slow.
bool needsBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const
void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
btOverlappingPairCallback * m_ghostPairCallback
void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)
void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
btBroadphasePairArray m_overlappingPairArray
void cleanProxyFromPairs(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
virtual void sortOverlappingPairs(btDispatcher *dispatcher)
The btBroadphasePair class contains a pair of aabb-overlapping objects.
btBroadphaseProxy * m_pProxy1
btBroadphaseProxy * m_pProxy0
btCollisionAlgorithm * m_algorithm
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:65
virtual bool processOverlap(btBroadphasePair &pair)=0