Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @ 8740

Last change on this file since 8740 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77

  • Property svn:eol-style set to native
File size: 15.3 KB
RevLine 
[1963]1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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"
[2882]22#include "LinearMath/btAabbUtil2.h"
[1963]23
24#include <stdio.h>
25
26int     gOverlappingPairs = 0;
27
28int gRemovePairs =0;
29int gAddedPairs =0;
30int gFindPairs =0;
31
32
33
34
35btHashedOverlappingPairCache::btHashedOverlappingPairCache():
36        m_overlapFilterCallback(0),
[2430]37        m_blockedForChanges(false),
38        m_ghostPairCallback(0)
[1963]39{
40        int initialAllocatedSize= 2;
41        m_overlappingPairArray.reserve(initialAllocatedSize);
42        growTables();
43}
44
45
46
47
48btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
49{
50}
51
52
53
54void    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
69void    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
106void    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
136btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
137{
138        gFindPairs++;
[2882]139        if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
140                btSwap(proxy0,proxy1);
[1963]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
172void    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
214btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
215{
[2882]216        if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
217                btSwap(proxy0,proxy1);
[1963]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();
[7983]243        void* mem = &m_overlappingPairArray.expandNonInitializing();
[2430]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
[1963]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;
[2430]262        pair->m_internalTmpValue = 0;
[1963]263       
264
265        m_next[count] = m_hashTable[hash];
266        m_hashTable[hash] = count;
267
268        return pair;
269}
270
271
272
273void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
274{
275        gRemovePairs++;
[2882]276        if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
277                btSwap(proxy0,proxy1);
[1963]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
[2430]294        void* userData = pair->m_internalInfo1;
[1963]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
[2430]329        if (m_ghostPairCallback)
330                m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
331
[1963]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
377void    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
[2882]399void    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        }
[1963]408
[2882]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        }
[1963]418
[2882]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
[1963]430void*   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];
[2430]441                        void* userData = pair.m_internalInfo1;
[1963]442                        cleanOverlappingPair(pair,dispatcher);
[2430]443                        if (m_ghostPairCallback)
444                                m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
[1963]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
462btBroadphasePair*       btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
463{
464        //don't add overlap with own
[2882]465        btAssert(proxy0 != proxy1);
[1963]466
467        if (!needsBroadphaseCollision(proxy0,proxy1))
468                return 0;
469       
[7983]470        void* mem = &m_overlappingPairArray.expandNonInitializing();
[1963]471        btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
[2430]472       
[1963]473        gOverlappingPairs++;
474        gAddedPairs++;
[2430]475       
476        if (m_ghostPairCallback)
477                m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
[1963]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        {
[2882]496                //btAssert(it != m_overlappingPairSet.end());
[1963]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
514void    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);
[2882]526                        pair->m_pProxy0 = 0;
527                        pair->m_pProxy1 = 0;
528                        m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
[1963]529                        m_overlappingPairArray.pop_back();
530                        gOverlappingPairs--;
531                } else
532                {
533                        i++;
534                }
535        }
536}
537
538
539
540
541btSortedOverlappingPairCache::btSortedOverlappingPairCache():
542        m_blockedForChanges(false),
543        m_hasDeferredRemoval(true),
[2430]544        m_overlapFilterCallback(0),
545        m_ghostPairCallback(0)
[1963]546{
547        int initialAllocatedSize= 2;
548        m_overlappingPairArray.reserve(initialAllocatedSize);
549}
550
551btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
552{
553}
554
555void    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
569void    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
604void    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}
[2882]628
629void    btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
630{
631        //should already be sorted
632}
633
Note: See TracBrowser for help on using the repository browser.