dali-physics/third-party/bullet3/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp

   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
  29 b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
  30 //,     m_blockedForChanges(false)
  31 {
  32         int initialAllocatedSize = 2;
  33         m_overlappingPairArray.reserve(initialAllocatedSize);
  34         growTables();
  35 }
  36
  37 b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
  38 {
  39 }
  40
  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
  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
  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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 484 b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
 485 {
 486 }
 487
 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
 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
 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
 556 void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
 557 {
 558         //should already be sorted
 559 }