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 
27 
28 int gRemovePairs =0;
29 int gAddedPairs =0;
30 int gFindPairs =0;
31 
32 
33 
34 
36  m_overlapFilterCallback(0),
37  m_ghostPairCallback(0)
38 {
39  int initialAllocatedSize= 2;
40  m_overlappingPairArray.reserve(initialAllocatedSize);
41  growTables();
42 }
43 
44 
45 
46 
48 {
49 }
50 
51 
52 
54 {
55  if (pair.m_algorithm && dispatcher)
56  {
57  {
59  dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
60  pair.m_algorithm=0;
61  }
62  }
63 }
64 
65 
66 
67 
69 {
70 
71  class CleanPairCallback : public btOverlapCallback
72  {
73  btBroadphaseProxy* m_cleanProxy;
74  btOverlappingPairCache* m_pairCache;
75  btDispatcher* m_dispatcher;
76 
77  public:
78  CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
79  :m_cleanProxy(cleanProxy),
80  m_pairCache(pairCache),
81  m_dispatcher(dispatcher)
82  {
83  }
84  virtual bool processOverlap(btBroadphasePair& pair)
85  {
86  if ((pair.m_pProxy0 == m_cleanProxy) ||
87  (pair.m_pProxy1 == m_cleanProxy))
88  {
89  m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
90  }
91  return false;
92  }
93 
94  };
95 
96  CleanPairCallback cleanPairs(proxy,this,dispatcher);
97 
98  processAllOverlappingPairs(&cleanPairs,dispatcher);
99 
100 }
101 
102 
103 
104 
106 {
107 
108  class RemovePairCallback : public btOverlapCallback
109  {
110  btBroadphaseProxy* m_obsoleteProxy;
111 
112  public:
113  RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
114  :m_obsoleteProxy(obsoleteProxy)
115  {
116  }
117  virtual bool processOverlap(btBroadphasePair& pair)
118  {
119  return ((pair.m_pProxy0 == m_obsoleteProxy) ||
120  (pair.m_pProxy1 == m_obsoleteProxy));
121  }
122 
123  };
124 
125 
126  RemovePairCallback removeCallback(proxy);
127 
128  processAllOverlappingPairs(&removeCallback,dispatcher);
129 }
130 
131 
132 
133 
134 
136 {
137  gFindPairs++;
138  if(proxy0->m_uniqueId>proxy1->m_uniqueId)
139  btSwap(proxy0,proxy1);
140  int proxyId1 = proxy0->getUid();
141  int proxyId2 = proxy1->getUid();
142 
143  /*if (proxyId1 > proxyId2)
144  btSwap(proxyId1, proxyId2);*/
145 
146  int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
147 
148  if (hash >= m_hashTable.size())
149  {
150  return NULL;
151  }
152 
153  int index = m_hashTable[hash];
154  while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
155  {
156  index = m_next[index];
157  }
158 
159  if (index == BT_NULL_PAIR)
160  {
161  return NULL;
162  }
163 
165 
166  return &m_overlappingPairArray[index];
167 }
168 
169 //#include <stdio.h>
170 
172 {
173 
174  int newCapacity = m_overlappingPairArray.capacity();
175 
176  if (m_hashTable.size() < newCapacity)
177  {
178  //grow hashtable and next table
179  int curHashtableSize = m_hashTable.size();
180 
181  m_hashTable.resize(newCapacity);
182  m_next.resize(newCapacity);
183 
184 
185  int i;
186 
187  for (i= 0; i < newCapacity; ++i)
188  {
190  }
191  for (i = 0; i < newCapacity; ++i)
192  {
193  m_next[i] = BT_NULL_PAIR;
194  }
195 
196  for(i=0;i<curHashtableSize;i++)
197  {
198 
199  const btBroadphasePair& pair = m_overlappingPairArray[i];
200  int proxyId1 = pair.m_pProxy0->getUid();
201  int proxyId2 = pair.m_pProxy1->getUid();
202  /*if (proxyId1 > proxyId2)
203  btSwap(proxyId1, proxyId2);*/
204  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
205  m_next[i] = m_hashTable[hashValue];
206  m_hashTable[hashValue] = i;
207  }
208 
209 
210  }
211 }
212 
214 {
215  if(proxy0->m_uniqueId>proxy1->m_uniqueId)
216  btSwap(proxy0,proxy1);
217  int proxyId1 = proxy0->getUid();
218  int proxyId2 = proxy1->getUid();
219 
220  /*if (proxyId1 > proxyId2)
221  btSwap(proxyId1, proxyId2);*/
222 
223  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
224 
225 
226  btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
227  if (pair != NULL)
228  {
229  return pair;
230  }
231  /*for(int i=0;i<m_overlappingPairArray.size();++i)
232  {
233  if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
234  (m_overlappingPairArray[i].m_pProxy1==proxy1))
235  {
236  printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
237  internalFindPair(proxy0, proxy1, hash);
238  }
239  }*/
240  int count = m_overlappingPairArray.size();
241  int oldCapacity = m_overlappingPairArray.capacity();
243 
244  //this is where we add an actual pair, so also call the 'ghost'
246  m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
247 
248  int newCapacity = m_overlappingPairArray.capacity();
249 
250  if (oldCapacity < newCapacity)
251  {
252  growTables();
253  //hash with new capacity
254  hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
255  }
256 
257  pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
258 // pair->m_pProxy0 = proxy0;
259 // pair->m_pProxy1 = proxy1;
260  pair->m_algorithm = 0;
261  pair->m_internalTmpValue = 0;
262 
263 
264  m_next[count] = m_hashTable[hash];
265  m_hashTable[hash] = count;
266 
267  return pair;
268 }
269 
270 
271 
273 {
274  gRemovePairs++;
275  if(proxy0->m_uniqueId>proxy1->m_uniqueId)
276  btSwap(proxy0,proxy1);
277  int proxyId1 = proxy0->getUid();
278  int proxyId2 = proxy1->getUid();
279 
280  /*if (proxyId1 > proxyId2)
281  btSwap(proxyId1, proxyId2);*/
282 
283  int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
284 
285  btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
286  if (pair == NULL)
287  {
288  return 0;
289  }
290 
291  cleanOverlappingPair(*pair,dispatcher);
292 
293  void* userData = pair->m_internalInfo1;
294 
295  btAssert(pair->m_pProxy0->getUid() == proxyId1);
296  btAssert(pair->m_pProxy1->getUid() == proxyId2);
297 
298  int pairIndex = int(pair - &m_overlappingPairArray[0]);
299  btAssert(pairIndex < m_overlappingPairArray.size());
300 
301  // Remove the pair from the hash table.
302  int index = m_hashTable[hash];
303  btAssert(index != BT_NULL_PAIR);
304 
305  int previous = BT_NULL_PAIR;
306  while (index != pairIndex)
307  {
308  previous = index;
309  index = m_next[index];
310  }
311 
312  if (previous != BT_NULL_PAIR)
313  {
314  btAssert(m_next[previous] == pairIndex);
315  m_next[previous] = m_next[pairIndex];
316  }
317  else
318  {
319  m_hashTable[hash] = m_next[pairIndex];
320  }
321 
322  // We now move the last pair into spot of the
323  // pair being removed. We need to fix the hash
324  // table indices to support the move.
325 
326  int lastPairIndex = m_overlappingPairArray.size() - 1;
327 
329  m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
330 
331  // If the removed pair is the last pair, we are done.
332  if (lastPairIndex == pairIndex)
333  {
335  return userData;
336  }
337 
338  // Remove the last pair from the hash table.
339  const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
340  /* missing swap here too, Nat. */
341  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));
342 
343  index = m_hashTable[lastHash];
344  btAssert(index != BT_NULL_PAIR);
345 
346  previous = BT_NULL_PAIR;
347  while (index != lastPairIndex)
348  {
349  previous = index;
350  index = m_next[index];
351  }
352 
353  if (previous != BT_NULL_PAIR)
354  {
355  btAssert(m_next[previous] == lastPairIndex);
356  m_next[previous] = m_next[lastPairIndex];
357  }
358  else
359  {
360  m_hashTable[lastHash] = m_next[lastPairIndex];
361  }
362 
363  // Copy the last pair into the remove pair's spot.
364  m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
365 
366  // Insert the last pair into the hash table
367  m_next[pairIndex] = m_hashTable[lastHash];
368  m_hashTable[lastHash] = pairIndex;
369 
371 
372  return userData;
373 }
374 //#include <stdio.h>
375 #include "LinearMath/btQuickprof.h"
377 {
378  BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
379  int i;
380 
381 // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
382  for (i=0;i<m_overlappingPairArray.size();)
383  {
384 
386  if (callback->processOverlap(*pair))
387  {
388  removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
389 
391  } else
392  {
393  i++;
394  }
395  }
396 }
397 
399 {
401  btBroadphasePairArray tmpPairs;
402  int i;
403  for (i=0;i<m_overlappingPairArray.size();i++)
404  {
405  tmpPairs.push_back(m_overlappingPairArray[i]);
406  }
407 
408  for (i=0;i<tmpPairs.size();i++)
409  {
410  removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
411  }
412 
413  for (i = 0; i < m_next.size(); i++)
414  {
415  m_next[i] = BT_NULL_PAIR;
416  }
417 
419 
420  for (i=0;i<tmpPairs.size();i++)
421  {
422  addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
423  }
424 
425 
426 }
427 
428 
430 {
431  if (!hasDeferredRemoval())
432  {
433  btBroadphasePair findPair(*proxy0,*proxy1);
434 
436  if (findIndex < m_overlappingPairArray.size())
437  {
439  btBroadphasePair& pair = m_overlappingPairArray[findIndex];
440  void* userData = pair.m_internalInfo1;
441  cleanOverlappingPair(pair,dispatcher);
443  m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
444 
447  return userData;
448  }
449  }
450 
451  return 0;
452 }
453 
454 
455 
456 
457 
458 
459 
460 
462 {
463  //don't add overlap with own
464  btAssert(proxy0 != proxy1);
465 
466  if (!needsBroadphaseCollision(proxy0,proxy1))
467  return 0;
468 
470  btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
471 
473  gAddedPairs++;
474 
476  m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
477  return pair;
478 
479 }
480 
486 {
487  if (!needsBroadphaseCollision(proxy0,proxy1))
488  return 0;
489 
490  btBroadphasePair tmpPair(*proxy0,*proxy1);
491  int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
492 
493  if (findIndex < m_overlappingPairArray.size())
494  {
495  //btAssert(it != m_overlappingPairSet.end());
496  btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
497  return pair;
498  }
499  return 0;
500 }
501 
502 
503 
504 
505 
506 
507 
508 
509 
510 
511 //#include <stdio.h>
512 
514 {
515 
516  int i;
517 
518  for (i=0;i<m_overlappingPairArray.size();)
519  {
520 
522  if (callback->processOverlap(*pair))
523  {
524  cleanOverlappingPair(*pair,dispatcher);
525  pair->m_pProxy0 = 0;
526  pair->m_pProxy1 = 0;
530  } else
531  {
532  i++;
533  }
534  }
535 }
536 
537 
538 
539 
541  m_blockedForChanges(false),
542  m_hasDeferredRemoval(true),
543  m_overlapFilterCallback(0),
544  m_ghostPairCallback(0)
545 {
546  int initialAllocatedSize= 2;
547  m_overlappingPairArray.reserve(initialAllocatedSize);
548 }
549 
551 {
552 }
553 
555 {
556  if (pair.m_algorithm)
557  {
558  {
560  dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
561  pair.m_algorithm=0;
562  gRemovePairs--;
563  }
564  }
565 }
566 
567 
569 {
570 
571  class CleanPairCallback : public btOverlapCallback
572  {
573  btBroadphaseProxy* m_cleanProxy;
574  btOverlappingPairCache* m_pairCache;
575  btDispatcher* m_dispatcher;
576 
577  public:
578  CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
579  :m_cleanProxy(cleanProxy),
580  m_pairCache(pairCache),
581  m_dispatcher(dispatcher)
582  {
583  }
584  virtual bool processOverlap(btBroadphasePair& pair)
585  {
586  if ((pair.m_pProxy0 == m_cleanProxy) ||
587  (pair.m_pProxy1 == m_cleanProxy))
588  {
589  m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
590  }
591  return false;
592  }
593 
594  };
595 
596  CleanPairCallback cleanPairs(proxy,this,dispatcher);
597 
598  processAllOverlappingPairs(&cleanPairs,dispatcher);
599 
600 }
601 
602 
604 {
605 
606  class RemovePairCallback : public btOverlapCallback
607  {
608  btBroadphaseProxy* m_obsoleteProxy;
609 
610  public:
611  RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
612  :m_obsoleteProxy(obsoleteProxy)
613  {
614  }
615  virtual bool processOverlap(btBroadphasePair& pair)
616  {
617  return ((pair.m_pProxy0 == m_obsoleteProxy) ||
618  (pair.m_pProxy1 == m_obsoleteProxy));
619  }
620 
621  };
622 
623  RemovePairCallback removeCallback(proxy);
624 
625  processAllOverlappingPairs(&removeCallback,dispatcher);
626 }
627 
629 {
630  //should already be sorted
631 }
632 
void push_back(const T &_Val)
virtual void processAllOverlappingPairs(btOverlapCallback *, btDispatcher *dispatcher)
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
const int BT_NULL_PAIR
int findLinearSearch(const T &key) const
btBroadphasePair * internalFindPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, int hash)
btAlignedObjectArray< int > m_hashTable
#define btAssert(x)
Definition: btScalar.h:131
btOverlappingPairCallback * m_ghostPairCallback
btBroadphasePairArray m_overlappingPairArray
bool equalsPair(const btBroadphasePair &pair, int proxyId1, int proxyId2)
btOverlappingPairCallback * m_ghostPairCallback
void cleanProxyFromPairs(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void swap(int index0, int index1)
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
virtual void sortOverlappingPairs(btDispatcher *dispatcher)
unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
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)
bool needsBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const
int gFindPairs
btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
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
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
int gOverlappingPairs
int size() const
return the number of elements in the array
btBroadphasePairArray m_overlappingPairArray
btBroadphaseProxy * m_pProxy0
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
virtual void sortOverlappingPairs(btDispatcher *dispatcher)
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
void resize(int newsize, const T &fillData=T())
void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
int gRemovePairs
int gAddedPairs
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:75
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.