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