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