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