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 #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 
btHashedSimplePairCache()27 btHashedSimplePairCache::btHashedSimplePairCache()
28 {
29 	int initialAllocatedSize = 2;
30 	m_overlappingPairArray.reserve(initialAllocatedSize);
31 	growTables();
32 }
33 
~btHashedSimplePairCache()34 btHashedSimplePairCache::~btHashedSimplePairCache()
35 {
36 }
37 
removeAllPairs()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 
findPair(int indexA,int indexB)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 
growTables()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 
internalAddPair(int indexA,int indexB)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 
removeOverlappingPair(int indexA,int indexB)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>
245