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