Merging trunk up to revision 41245.
[blender.git] / extern / bullet2 / src / BulletCollision / BroadphaseCollision / btOverlappingPairCache.cpp
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
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
48 btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
49 {
50 }
51
52
53
54 void    btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
55 {
56         if (pair.m_algorithm)
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
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
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
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
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
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
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
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
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
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
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
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
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
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
551 btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
552 {
553 }
554
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
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
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
629 void    btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
630 {
631         //should already be sorted
632 }
633