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 
17 
18 #include "btOverlappingPairCache.h"
19 
20 #include "btDispatcher.h"
21 #include "btCollisionAlgorithm.h"
22 #include "LinearMath/btAabbUtil2.h"
23 
24 #include <stdio.h>
25 
26 
27 
28 
29 
31  m_overlapFilterCallback(0),
32  m_ghostPairCallback(0)
33 {
34  int initialAllocatedSize= 2;
35  m_overlappingPairArray.reserve(initialAllocatedSize);
36  growTables();
37 }
38 
39 
40 
41 
43 {
44 }
45 
46 
47 
49 {
50  if (pair.m_algorithm && dispatcher)
51  {
52  {
54  dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
55  pair.m_algorithm=0;
56  }
57  }
58 }
59 
60 
61 
62 
64 {
65 
66  class CleanPairCallback : public btOverlapCallback
67  {
68  btBroadphaseProxy* m_cleanProxy;
69  btOverlappingPairCache* m_pairCache;
70  btDispatcher* m_dispatcher;
71 
72  public:
73  CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
74  :m_cleanProxy(cleanProxy),
75  m_pairCache(pairCache),
76  m_dispatcher(dispatcher)
77  {
78  }
79  virtual bool processOverlap(btBroadphasePair& pair)
80  {
81  if ((pair.m_pProxy0 == m_cleanProxy) ||
82  (pair.m_pProxy1 == m_cleanProxy))
83  {
84  m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
85  }
86  return false;
87  }
88 
89  };
90 
91  CleanPairCallback cleanPairs(proxy,this,dispatcher);
92 
93  processAllOverlappingPairs(&cleanPairs,dispatcher);
94 
95 }
96 
97 
98 
99 
101 {
102 
103  class RemovePairCallback : public btOverlapCallback
104  {
105  btBroadphaseProxy* m_obsoleteProxy;
106 
107  public:
108  RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
109  :m_obsoleteProxy(obsoleteProxy)
110  {
111  }
112  virtual bool processOverlap(btBroadphasePair& pair)
113  {
114  return ((pair.m_pProxy0 == m_obsoleteProxy) ||
115  (pair.m_pProxy1 == m_obsoleteProxy));
116  }
117 
118  };
119 
120 
121  RemovePairCallback removeCallback(proxy);
122 
123  processAllOverlappingPairs(&removeCallback,dispatcher);
124 }
125 
126 
127 
128 
129 
131 {
132  if(proxy0->m_uniqueId>proxy1->m_uniqueId)
133  btSwap(proxy0,proxy1);
134  int proxyId1 = proxy0->getUid();
135  int proxyId2 = proxy1->getUid();
136 
137  /*if (proxyId1 > proxyId2)
138  btSwap(proxyId1, proxyId2);*/
139 
140  int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
141 
142  if (hash >= m_hashTable.size())
143  {
144  return NULL;
145  }
146 
147  int index = m_hashTable[hash];
148  while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
149  {
150  index = m_next[index];
151  }
152 
153  if (index == BT_NULL_PAIR)
154  {
155  return NULL;
156  }
157 
159 
160  return &m_overlappingPairArray[index];
161 }
162 
163 //#include <stdio.h>
164 
166 {
167 
168  int newCapacity = m_overlappingPairArray.capacity();
169 
170  if (m_hashTable.size() < newCapacity)
171  {
172  //grow hashtable and next table
173  int curHashtableSize = m_hashTable.size();
174 
175  m_hashTable.resize(newCapacity);
176  m_next.resize(newCapacity);
177 
178 
179  int i;
180 
181  for (i= 0; i < newCapacity; ++i)
182  {
184  }
185  for (i = 0; i < newCapacity; ++i)
186  {
187  m_next[i] = BT_NULL_PAIR;
188  }
189 
190  for(i=0;i<curHashtableSize;i++)
191  {
192 
193  const btBroadphasePair& pair = m_overlappingPairArray[i];
194  int proxyId1 = pair.m_pProxy0->getUid();
195  int proxyId2 = pair.m_pProxy1->getUid();
196  /*if (proxyId1 > proxyId2)
197  btSwap(proxyId1, proxyId2);*/
198  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
199  m_next[i] = m_hashTable[hashValue];
200  m_hashTable[hashValue] = i;
201  }
202 
203 
204  }
205 }
206 
208 {
209  if(proxy0->m_uniqueId>proxy1->m_uniqueId)
210  btSwap(proxy0,proxy1);
211  int proxyId1 = proxy0->getUid();
212  int proxyId2 = proxy1->getUid();
213 
214  /*if (proxyId1 > proxyId2)
215  btSwap(proxyId1, proxyId2);*/
216 
217  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
218 
219 
220  btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
221  if (pair != NULL)
222  {
223  return pair;
224  }
225  /*for(int i=0;i<m_overlappingPairArray.size();++i)
226  {
227  if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
228  (m_overlappingPairArray[i].m_pProxy1==proxy1))
229  {
230  printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
231  internalFindPair(proxy0, proxy1, hash);
232  }
233  }*/
234  int count = m_overlappingPairArray.size();
235  int oldCapacity = m_overlappingPairArray.capacity();
237 
238  //this is where we add an actual pair, so also call the 'ghost'
240  m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
241 
242  int newCapacity = m_overlappingPairArray.capacity();
243 
244  if (oldCapacity < newCapacity)
245  {
246  growTables();
247  //hash with new capacity
248  hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
249  }
250 
251  pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
252 // pair->m_pProxy0 = proxy0;
253 // pair->m_pProxy1 = proxy1;
254  pair->m_algorithm = 0;
255  pair->m_internalTmpValue = 0;
256 
257 
258  m_next[count] = m_hashTable[hash];
259  m_hashTable[hash] = count;
260 
261  return pair;
262 }
263 
264 
265 
267 {
268  if(proxy0->m_uniqueId>proxy1->m_uniqueId)
269  btSwap(proxy0,proxy1);
270  int proxyId1 = proxy0->getUid();
271  int proxyId2 = proxy1->getUid();
272 
273  /*if (proxyId1 > proxyId2)
274  btSwap(proxyId1, proxyId2);*/
275 
276  int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
277 
278  btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
279  if (pair == NULL)
280  {
281  return 0;
282  }
283 
284  cleanOverlappingPair(*pair,dispatcher);
285 
286  void* userData = pair->m_internalInfo1;
287 
288  btAssert(pair->m_pProxy0->getUid() == proxyId1);
289  btAssert(pair->m_pProxy1->getUid() == proxyId2);
290 
291  int pairIndex = int(pair - &m_overlappingPairArray[0]);
292  btAssert(pairIndex < m_overlappingPairArray.size());
293 
294  // Remove the pair from the hash table.
295  int index = m_hashTable[hash];
296  btAssert(index != BT_NULL_PAIR);
297 
298  int previous = BT_NULL_PAIR;
299  while (index != pairIndex)
300  {
301  previous = index;
302  index = m_next[index];
303  }
304 
305  if (previous != BT_NULL_PAIR)
306  {
307  btAssert(m_next[previous] == pairIndex);
308  m_next[previous] = m_next[pairIndex];
309  }
310  else
311  {
312  m_hashTable[hash] = m_next[pairIndex];
313  }
314 
315  // We now move the last pair into spot of the
316  // pair being removed. We need to fix the hash
317  // table indices to support the move.
318 
319  int lastPairIndex = m_overlappingPairArray.size() - 1;
320 
322  m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
323 
324  // If the removed pair is the last pair, we are done.
325  if (lastPairIndex == pairIndex)
326  {
328  return userData;
329  }
330 
331  // Remove the last pair from the hash table.
332  const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
333  /* missing swap here too, Nat. */
334  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));
335 
336  index = m_hashTable[lastHash];
337  btAssert(index != BT_NULL_PAIR);
338 
339  previous = BT_NULL_PAIR;
340  while (index != lastPairIndex)
341  {
342  previous = index;
343  index = m_next[index];
344  }
345 
346  if (previous != BT_NULL_PAIR)
347  {
348  btAssert(m_next[previous] == lastPairIndex);
349  m_next[previous] = m_next[lastPairIndex];
350  }
351  else
352  {
353  m_hashTable[lastHash] = m_next[lastPairIndex];
354  }
355 
356  // Copy the last pair into the remove pair's spot.
357  m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
358 
359  // Insert the last pair into the hash table
360  m_next[pairIndex] = m_hashTable[lastHash];
361  m_hashTable[lastHash] = pairIndex;
362 
364 
365  return userData;
366 }
367 //#include <stdio.h>
368 #include "LinearMath/btQuickprof.h"
370 {
371  BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
372  int i;
373 
374 // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
375  for (i=0;i<m_overlappingPairArray.size();)
376  {
377 
379  if (callback->processOverlap(*pair))
380  {
381  removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
382  } else
383  {
384  i++;
385  }
386  }
387 }
388 
390 {
392  int m_uidA0;
393  int m_uidA1;
394 };
395 
397 {
398 public:
399 
400  bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const
401  {
402  const int uidA0 = a.m_uidA0;
403  const int uidB0 = b.m_uidA0;
404  const int uidA1 = a.m_uidA1;
405  const int uidB1 = b.m_uidA1;
406  return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
407  }
408 };
409 
411 {
412  if (dispatchInfo.m_deterministicOverlappingPairs)
413  {
416  {
417  BT_PROFILE("sortOverlappingPairs");
418  indices.resize(pa.size());
419  for (int i=0;i<indices.size();i++)
420  {
421  const btBroadphasePair& p = pa[i];
422  const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
423  const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
424 
425  indices[i].m_uidA0 = uidA0;
426  indices[i].m_uidA1 = uidA1;
427  indices[i].m_orgIndex = i;
428  }
430  }
431  {
432  BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
433  int i;
434  for (i=0;i<indices.size();)
435  {
436  btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
437  if (callback->processOverlap(*pair))
438  {
439  removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
440  } else
441  {
442  i++;
443  }
444  }
445  }
446  } else
447  {
448  processAllOverlappingPairs(callback, dispatcher);
449  }
450 }
451 
452 
454 {
456  btBroadphasePairArray tmpPairs;
457  int i;
458  for (i=0;i<m_overlappingPairArray.size();i++)
459  {
460  tmpPairs.push_back(m_overlappingPairArray[i]);
461  }
462 
463  for (i=0;i<tmpPairs.size();i++)
464  {
465  removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
466  }
467 
468  for (i = 0; i < m_next.size(); i++)
469  {
470  m_next[i] = BT_NULL_PAIR;
471  }
472 
474 
475  for (i=0;i<tmpPairs.size();i++)
476  {
477  addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
478  }
479 
480 
481 }
482 
483 
485 {
486  if (!hasDeferredRemoval())
487  {
488  btBroadphasePair findPair(*proxy0,*proxy1);
489 
490  int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
491  if (findIndex < m_overlappingPairArray.size())
492  {
493  btBroadphasePair& pair = m_overlappingPairArray[findIndex];
494  void* userData = pair.m_internalInfo1;
495  cleanOverlappingPair(pair,dispatcher);
497  m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
498 
501  return userData;
502  }
503  }
504 
505  return 0;
506 }
507 
508 
509 
510 
511 
512 
513 
514 
516 {
517  //don't add overlap with own
518  btAssert(proxy0 != proxy1);
519 
520  if (!needsBroadphaseCollision(proxy0,proxy1))
521  return 0;
522 
524  btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
525 
527  m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
528  return pair;
529 
530 }
531 
537 {
538  if (!needsBroadphaseCollision(proxy0,proxy1))
539  return 0;
540 
541  btBroadphasePair tmpPair(*proxy0,*proxy1);
542  int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
543 
544  if (findIndex < m_overlappingPairArray.size())
545  {
546  //btAssert(it != m_overlappingPairSet.end());
547  btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
548  return pair;
549  }
550  return 0;
551 }
552 
553 
554 
555 
556 
557 
558 
559 
560 
561 
562 //#include <stdio.h>
563 
565 {
566 
567  int i;
568 
569  for (i=0;i<m_overlappingPairArray.size();)
570  {
571 
573  if (callback->processOverlap(*pair))
574  {
575  cleanOverlappingPair(*pair,dispatcher);
576  pair->m_pProxy0 = 0;
577  pair->m_pProxy1 = 0;
580  } else
581  {
582  i++;
583  }
584  }
585 }
586 
587 
588 
589 
591  m_blockedForChanges(false),
592  m_hasDeferredRemoval(true),
595 {
596  int initialAllocatedSize= 2;
597  m_overlappingPairArray.reserve(initialAllocatedSize);
598 }
599 
601 {
602 }
603 
605 {
606  if (pair.m_algorithm)
607  {
608  {
610  dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
611  pair.m_algorithm=0;
612  }
613  }
614 }
615 
616 
618 {
619 
620  class CleanPairCallback : public btOverlapCallback
621  {
622  btBroadphaseProxy* m_cleanProxy;
623  btOverlappingPairCache* m_pairCache;
624  btDispatcher* m_dispatcher;
625 
626  public:
627  CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
628  :m_cleanProxy(cleanProxy),
629  m_pairCache(pairCache),
630  m_dispatcher(dispatcher)
631  {
632  }
633  virtual bool processOverlap(btBroadphasePair& pair)
634  {
635  if ((pair.m_pProxy0 == m_cleanProxy) ||
636  (pair.m_pProxy1 == m_cleanProxy))
637  {
638  m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
639  }
640  return false;
641  }
642 
643  };
644 
645  CleanPairCallback cleanPairs(proxy,this,dispatcher);
646 
647  processAllOverlappingPairs(&cleanPairs,dispatcher);
648 
649 }
650 
651 
653 {
654 
655  class RemovePairCallback : public btOverlapCallback
656  {
657  btBroadphaseProxy* m_obsoleteProxy;
658 
659  public:
660  RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
661  :m_obsoleteProxy(obsoleteProxy)
662  {
663  }
664  virtual bool processOverlap(btBroadphasePair& pair)
665  {
666  return ((pair.m_pProxy0 == m_obsoleteProxy) ||
667  (pair.m_pProxy1 == m_obsoleteProxy));
668  }
669 
670  };
671 
672  RemovePairCallback removeCallback(proxy);
673 
674  processAllOverlappingPairs(&removeCallback,dispatcher);
675 }
676 
678 {
679  //should already be sorted
680 }
681 
void push_back(const T &_Val)
virtual void processAllOverlappingPairs(btOverlapCallback *, btDispatcher *dispatcher)
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
bool needsBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const
const int BT_NULL_PAIR
btBroadphasePair * internalFindPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, int hash)
btAlignedObjectArray< int > m_hashTable
#define btAssert(x)
Definition: btScalar.h:131
btBroadphasePairArray & getOverlappingPairArray()
btBroadphasePairArray m_overlappingPairArray
bool equalsPair(const btBroadphasePair &pair, int proxyId1, int proxyId2)
btOverlappingPairCallback * m_ghostPairCallback
btOverlapFilterCallback * m_overlapFilterCallback
void cleanProxyFromPairs(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void swap(int index0, int index1)
The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
virtual void sortOverlappingPairs(btDispatcher *dispatcher)
unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
int size() const
return the number of elements in the array
btBroadphasePair * internalAddPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
void btSwap(T &a, T &b)
Definition: btScalar.h:621
void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)
btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual bool processOverlap(btBroadphasePair &pair)=0
void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
btAlignedObjectArray< int > m_next
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
btBroadphaseProxy * m_pProxy1
btCollisionAlgorithm * m_algorithm
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
btBroadphasePairArray m_overlappingPairArray
btBroadphaseProxy * m_pProxy0
#define BT_PROFILE(name)
Definition: btQuickprof.h:216
virtual void sortOverlappingPairs(btDispatcher *dispatcher)
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
void resize(int newsize, const T &fillData=T())
int findLinearSearch(const T &key) const
void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:66
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
virtual void processAllOverlappingPairs(btOverlapCallback *, btDispatcher *dispatcher)
void cleanProxyFromPairs(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
this findPair becomes really slow.
void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)
void quickSort(const L &CompareFunc)
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
The btBroadphasePair class contains a pair of aabb-overlapping objects.