dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://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 "btHashedSimplePairCache.h"
  17
  18 #include <stdio.h>
  19
  20 #ifdef BT_DEBUG_COLLISION_PAIRS
  21 int gOverlappingSimplePairs = 0;
  22 int gRemoveSimplePairs = 0;
  23 int gAddedSimplePairs = 0;
  24 int gFindSimplePairs = 0;
  25 #endif  //BT_DEBUG_COLLISION_PAIRS
  26
  27 btHashedSimplePairCache::btHashedSimplePairCache()
  28 {
  29         int initialAllocatedSize = 2;
  30         m_overlappingPairArray.reserve(initialAllocatedSize);
  31         growTables();
  32 }
  33
  34 btHashedSimplePairCache::~btHashedSimplePairCache()
  35 {
  36 }
  37
  38 void btHashedSimplePairCache::removeAllPairs()
  39 {
  40         m_overlappingPairArray.clear();
  41         m_hashTable.clear();
  42         m_next.clear();
  43
  44         int initialAllocatedSize = 2;
  45         m_overlappingPairArray.reserve(initialAllocatedSize);
  46         growTables();
  47 }
  48
  49 btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
  50 {
  51 #ifdef BT_DEBUG_COLLISION_PAIRS
  52         gFindSimplePairs++;
  53 #endif
  54
  55         /*if (indexA > indexB)
  56                 btSwap(indexA, indexB);*/
  57
  58         int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
  59
  60         if (hash >= m_hashTable.size())
  61         {
  62                 return NULL;
  63         }
  64
  65         int index = m_hashTable[hash];
  66         while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false)
  67         {
  68                 index = m_next[index];
  69         }
  70
  71         if (index == BT_SIMPLE_NULL_PAIR)
  72         {
  73                 return NULL;
  74         }
  75
  76         btAssert(index < m_overlappingPairArray.size());
  77
  78         return &m_overlappingPairArray[index];
  79 }
  80
  81 //#include <stdio.h>
  82
  83 void btHashedSimplePairCache::growTables()
  84 {
  85         int newCapacity = m_overlappingPairArray.capacity();
  86
  87         if (m_hashTable.size() < newCapacity)
  88         {
  89                 //grow hashtable and next table
  90                 int curHashtableSize = m_hashTable.size();
  91
  92                 m_hashTable.resize(newCapacity);
  93                 m_next.resize(newCapacity);
  94
  95                 int i;
  96
  97                 for (i = 0; i < newCapacity; ++i)
  98                 {
  99                         m_hashTable[i] = BT_SIMPLE_NULL_PAIR;
 100                 }
 101                 for (i = 0; i < newCapacity; ++i)
 102                 {
 103                         m_next[i] = BT_SIMPLE_NULL_PAIR;
 104                 }
 105
 106                 for (i = 0; i < curHashtableSize; i++)
 107                 {
 108                         const btSimplePair& pair = m_overlappingPairArray[i];
 109                         int indexA = pair.m_indexA;
 110                         int indexB = pair.m_indexB;
 111
 112                         int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 113                         m_next[i] = m_hashTable[hashValue];
 114                         m_hashTable[hashValue] = i;
 115                 }
 116         }
 117 }
 118
 119 btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
 120 {
 121         int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 122
 123         btSimplePair* pair = internalFindPair(indexA, indexB, hash);
 124         if (pair != NULL)
 125         {
 126                 return pair;
 127         }
 128
 129         int count = m_overlappingPairArray.size();
 130         int oldCapacity = m_overlappingPairArray.capacity();
 131         void* mem = &m_overlappingPairArray.expandNonInitializing();
 132
 133         int newCapacity = m_overlappingPairArray.capacity();
 134
 135         if (oldCapacity < newCapacity)
 136         {
 137                 growTables();
 138                 //hash with new capacity
 139                 hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
 140         }
 141
 142         pair = new (mem) btSimplePair(indexA, indexB);
 143
 144         pair->m_userPointer = 0;
 145
 146         m_next[count] = m_hashTable[hash];
 147         m_hashTable[hash] = count;
 148
 149         return pair;
 150 }
 151
 152 void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
 153 {
 154 #ifdef BT_DEBUG_COLLISION_PAIRS
 155         gRemoveSimplePairs++;
 156 #endif
 157
 158         /*if (indexA > indexB)
 159                 btSwap(indexA, indexB);*/
 160
 161         int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
 162
 163         btSimplePair* pair = internalFindPair(indexA, indexB, hash);
 164         if (pair == NULL)
 165         {
 166                 return 0;
 167         }
 168
 169         void* userData = pair->m_userPointer;
 170
 171         int pairIndex = int(pair - &m_overlappingPairArray[0]);
 172         btAssert(pairIndex < m_overlappingPairArray.size());
 173
 174         // Remove the pair from the hash table.
 175         int index = m_hashTable[hash];
 176         btAssert(index != BT_SIMPLE_NULL_PAIR);
 177
 178         int previous = BT_SIMPLE_NULL_PAIR;
 179         while (index != pairIndex)
 180         {
 181                 previous = index;
 182                 index = m_next[index];
 183         }
 184
 185         if (previous != BT_SIMPLE_NULL_PAIR)
 186         {
 187                 btAssert(m_next[previous] == pairIndex);
 188                 m_next[previous] = m_next[pairIndex];
 189         }
 190         else
 191         {
 192                 m_hashTable[hash] = m_next[pairIndex];
 193         }
 194
 195         // We now move the last pair into spot of the
 196         // pair being removed. We need to fix the hash
 197         // table indices to support the move.
 198
 199         int lastPairIndex = m_overlappingPairArray.size() - 1;
 200
 201         // If the removed pair is the last pair, we are done.
 202         if (lastPairIndex == pairIndex)
 203         {
 204                 m_overlappingPairArray.pop_back();
 205                 return userData;
 206         }
 207
 208         // Remove the last pair from the hash table.
 209         const btSimplePair* last = &m_overlappingPairArray[lastPairIndex];
 210         /* missing swap here too, Nat. */
 211         int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity() - 1));
 212
 213         index = m_hashTable[lastHash];
 214         btAssert(index != BT_SIMPLE_NULL_PAIR);
 215
 216         previous = BT_SIMPLE_NULL_PAIR;
 217         while (index != lastPairIndex)
 218         {
 219                 previous = index;
 220                 index = m_next[index];
 221         }
 222
 223         if (previous != BT_SIMPLE_NULL_PAIR)
 224         {
 225                 btAssert(m_next[previous] == lastPairIndex);
 226                 m_next[previous] = m_next[lastPairIndex];
 227         }
 228         else
 229         {
 230                 m_hashTable[lastHash] = m_next[lastPairIndex];
 231         }
 232
 233         // Copy the last pair into the remove pair's spot.
 234         m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
 235
 236         // Insert the last pair into the hash table
 237         m_next[pairIndex] = m_hashTable[lastHash];
 238         m_hashTable[lastHash] = pairIndex;
 239
 240         m_overlappingPairArray.pop_back();
 241
 242         return userData;
 243 }
 244 //#include <stdio.h>