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