Blender  V3.3
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 
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  {
41  pair.m_algorithm->~btCollisionAlgorithm();
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;
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 
130  btAssert(index < m_overlappingPairArray.size());
131 
132  return &m_overlappingPairArray[index];
133 }
134 
135 //#include <stdio.h>
136 
137 void btHashedOverlappingPairCache::growTables()
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 
174 btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
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();
202  void* mem = &m_overlappingPairArray.expandNonInitializing();
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 
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  {
290  m_overlappingPairArray.pop_back();
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 
326  m_overlappingPairArray.pop_back();
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  {
340  btBroadphasePair* pair = &m_overlappingPairArray[i];
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  }
391  indices.quickSort(MyPairIndeSortPredicate());
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  {
413  }
414 }
415 
416 void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
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 
450  int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
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 
459  m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
460  m_overlappingPairArray.pop_back();
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 
476  void* mem = &m_overlappingPairArray.expandNonInitializing();
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  {
513  btBroadphasePair* pair = &m_overlappingPairArray[i];
514  if (callback->processOverlap(*pair))
515  {
516  cleanOverlappingPair(*pair, dispatcher);
517  pair->m_pProxy0 = 0;
518  pair->m_pProxy1 = 0;
519  m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
520  m_overlappingPairArray.pop_back();
521  }
522  else
523  {
524  i++;
525  }
526  }
527 }
528 
529 btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false),
530  m_hasDeferredRemoval(true),
533 {
534  int initialAllocatedSize = 2;
535  m_overlappingPairArray.reserve(initialAllocatedSize);
536 }
537 
538 btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
539 {
540 }
541 
543 {
544  if (pair.m_algorithm)
545  {
546  {
547  pair.m_algorithm->~btCollisionAlgorithm();
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;
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 
608 void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
609 {
610  //should already be sorted
611 }
btBroadphasePair
btBroadphaseProxy
btBroadphaseProxy * m_pProxy0
btBroadphaseProxy * m_pProxy1
btDispatcher * m_dispatcher
SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const
virtual ~btHashedOverlappingPairCache()
btAlignedObjectArray< int > m_next
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)
virtual void processAllOverlappingPairs(btOverlapCallback *, btDispatcher *dispatcher)
btOverlapFilterCallback * m_overlapFilterCallback
btHashedOverlappingPairCache()
void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)
btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
btOverlappingPairCallback * m_ghostPairCallback
btBroadphasePairArray & getOverlappingPairArray()
void cleanProxyFromPairs(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
const int BT_NULL_PAIR
btAlignedObjectArray< int > m_hashTable
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
SIMD_FORCE_INLINE void btSwap(T &a, T &b)
Definition: btScalar.h:643
#define btAssert(x)
Definition: btScalar.h:295
bool operator()(const MyPairIndex &a, const MyPairIndex &b) const
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void pop_back()
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
SIMD_FORCE_INLINE void push_back(const T &_Val)
virtual void freeCollisionAlgorithm(void *ptr)=0
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
DEGForeachIDComponentCallback callback
int count
ccl_gpu_kernel_postfix int ccl_global int * indices
static unsigned a[3]
Definition: RandGen.cpp:78
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
#define hash
Definition: noise.c:153
bool m_deterministicOverlappingPairs
Definition: btDispatcher.h:65
virtual bool processOverlap(btBroadphasePair &pair)=0