Bullet Collision Detection & Physics Library: btSimpleBroadphase.cpp Source File

Bullet Collision Detection & Physics Library

btSimpleBroadphase.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include "btSimpleBroadphase.h"
17#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
18#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
19
20#include "LinearMath/btVector3.h"
21#include "LinearMath/btTransform.h"
22#include "LinearMath/btMatrix3x3.h"
23#include "LinearMath/btAabbUtil2.h"
24
25#include <new>
26
27 void btSimpleBroadphase::validate()
28{
29 for (int i = 0; i < m_numHandles; i++)
30 {
31 for (int j = i + 1; j < m_numHandles; j++)
32 {
33 btAssert(&m_pHandles[i] != &m_pHandles[j]);
34 }
35 }
36}
37
38btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
39 : m_pairCache(overlappingPairCache),
40 m_ownsPairCache(false),
41 m_invalidPair(0)
42{
43 if (!overlappingPairCache)
44 {
45 void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
46 m_pairCache = new (mem) btHashedOverlappingPairCache();
47 m_ownsPairCache = true;
48 }
49
50 // allocate handles buffer and put all handles on free list
51 m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * maxProxies, 16);
52 m_pHandles = new (m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
53 m_maxHandles = maxProxies;
54 m_numHandles = 0;
55 m_firstFreeHandle = 0;
56 m_LastHandleIndex = -1;
57
58 {
59 for (int i = m_firstFreeHandle; i < maxProxies; i++)
60 {
61 m_pHandles[i].SetNextFree(i + 1);
62 m_pHandles[i].m_uniqueId = i + 2; //any UID will do, we just avoid too trivial values (0,1) for debugging purposes
63 }
64 m_pHandles[maxProxies - 1].SetNextFree(0);
65 }
66}
67
68btSimpleBroadphase::~btSimpleBroadphase()
69{
70 btAlignedFree(m_pHandlesRawPtr);
71
72 if (m_ownsPairCache)
73 {
74 m_pairCache->~btOverlappingPairCache();
75 btAlignedFree(m_pairCache);
76 }
77}
78
79 btBroadphaseProxy* btSimpleBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
80{
81 if (m_numHandles >= m_maxHandles)
82 {
83 btAssert(0);
84 return 0; //should never happen, but don't let the game crash ;-)
85 }
86 btAssert(aabbMin[0] <= aabbMax[0] && aabbMin[1] <= aabbMax[1] && aabbMin[2] <= aabbMax[2]);
87
88 int newHandleIndex = allocHandle();
89 btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex]) btSimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask);
90
91 return proxy;
92}
93
94 class RemovingOverlapCallback : public btOverlapCallback
95{
96protected:
97 virtual bool processOverlap(btBroadphasePair& pair)
98 {
99 (void)pair;
100 btAssert(0);
101 return false;
102 }
103};
104
105 class RemovePairContainingProxy
106{
107 btBroadphaseProxy* m_targetProxy;
108
109public:
110 virtual ~RemovePairContainingProxy()
111 {
112 }
113
114protected:
115 virtual bool processOverlap(btBroadphasePair& pair)
116 {
117 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
118 btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
119
120 return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
121 };
122};
123
124 void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher)
125{
126 m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
127
128 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
129 freeHandle(proxy0);
130
131 //validate();
132}
133
134 void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
135{
136 const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
137 aabbMin = sbp->m_aabbMin;
138 aabbMax = sbp->m_aabbMax;
139}
140
141 void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
142{
143 btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
144 sbp->m_aabbMin = aabbMin;
145 sbp->m_aabbMax = aabbMax;
146}
147
148 void btSimpleBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
149{
150 for (int i = 0; i <= m_LastHandleIndex; i++)
151 {
152 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
153 if (!proxy->m_clientObject)
154 {
155 continue;
156 }
157 rayCallback.process(proxy);
158 }
159}
160
161 void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
162{
163 for (int i = 0; i <= m_LastHandleIndex; i++)
164 {
165 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
166 if (!proxy->m_clientObject)
167 {
168 continue;
169 }
170 if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax))
171 {
172 callback.process(proxy);
173 }
174 }
175}
176
177 bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1)
178{
179 return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
180 proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
181 proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
182}
183
184//then remove non-overlapping ones
185 class CheckOverlapCallback : public btOverlapCallback
186{
187public:
188 virtual bool processOverlap(btBroadphasePair& pair)
189 {
190 return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0), static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
191 }
192};
193
194 void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
195{
196 //first check for new overlapping pairs
197 int i, j;
198 if (m_numHandles >= 0)
199 {
200 int new_largest_index = -1;
201 for (i = 0; i <= m_LastHandleIndex; i++)
202 {
203 btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
204 if (!proxy0->m_clientObject)
205 {
206 continue;
207 }
208 new_largest_index = i;
209 for (j = i + 1; j <= m_LastHandleIndex; j++)
210 {
211 btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
212 btAssert(proxy0 != proxy1);
213 if (!proxy1->m_clientObject)
214 {
215 continue;
216 }
217
218 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
219 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
220
221 if (aabbOverlap(p0, p1))
222 {
223 if (!m_pairCache->findPair(proxy0, proxy1))
224 {
225 m_pairCache->addOverlappingPair(proxy0, proxy1);
226 }
227 }
228 else
229 {
230 if (!m_pairCache->hasDeferredRemoval())
231 {
232 if (m_pairCache->findPair(proxy0, proxy1))
233 {
234 m_pairCache->removeOverlappingPair(proxy0, proxy1, dispatcher);
235 }
236 }
237 }
238 }
239 }
240
241 m_LastHandleIndex = new_largest_index;
242
243 if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
244 {
245 btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
246
247 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
248 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
249
250 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
251 m_invalidPair = 0;
252
253 btBroadphasePair previousPair;
254 previousPair.m_pProxy0 = 0;
255 previousPair.m_pProxy1 = 0;
256 previousPair.m_algorithm = 0;
257
258 for (i = 0; i < overlappingPairArray.size(); i++)
259 {
260 btBroadphasePair& pair = overlappingPairArray[i];
261
262 bool isDuplicate = (pair == previousPair);
263
264 previousPair = pair;
265
266 bool needsRemoval = false;
267
268 if (!isDuplicate)
269 {
270 bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
271
272 if (hasOverlap)
273 {
274 needsRemoval = false; //callback->processOverlap(pair);
275 }
276 else
277 {
278 needsRemoval = true;
279 }
280 }
281 else
282 {
283 //remove duplicate
284 needsRemoval = true;
285 //should have no algorithm
286 btAssert(!pair.m_algorithm);
287 }
288
289 if (needsRemoval)
290 {
291 m_pairCache->cleanOverlappingPair(pair, dispatcher);
292
293 // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
294 // m_overlappingPairArray.pop_back();
295 pair.m_pProxy0 = 0;
296 pair.m_pProxy1 = 0;
297 m_invalidPair++;
298 }
299 }
300
302#define CLEAN_INVALID_PAIRS 1
303#ifdef CLEAN_INVALID_PAIRS
304
305 //perform a sort, to sort 'invalid' pairs to the end
306 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
307
308 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
309 m_invalidPair = 0;
310#endif //CLEAN_INVALID_PAIRS
311 }
312 }
313}
314
315 bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
316{
317 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
318 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
319 return aabbOverlap(p0, p1);
320}
321
322 void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
323{
324 //not yet
325}
btAabbUtil2.h
TestAabbAgainstAabb2
bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
Definition: btAabbUtil2.h:43
btAlignedFree
#define btAlignedFree(ptr)
Definition: btAlignedAllocator.h:47
btAlignedAlloc
#define btAlignedAlloc(size, alignment)
Definition: btAlignedAllocator.h:46
btCollisionAlgorithm.h
btDispatcher.h
btMatrix3x3.h
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSimpleBroadphase.h
btTransform.h
btVector3.h
CheckOverlapCallback
Definition: btSimpleBroadphase.cpp:186
CheckOverlapCallback::processOverlap
virtual bool processOverlap(btBroadphasePair &pair)
Definition: btSimpleBroadphase.cpp:188
RemovePairContainingProxy
Definition: btSimpleBroadphase.cpp:106
RemovePairContainingProxy::m_targetProxy
btBroadphaseProxy * m_targetProxy
Definition: btSimpleBroadphase.cpp:107
RemovePairContainingProxy::~RemovePairContainingProxy
virtual ~RemovePairContainingProxy()
Definition: btSimpleBroadphase.cpp:110
RemovePairContainingProxy::processOverlap
virtual bool processOverlap(btBroadphasePair &pair)
Definition: btSimpleBroadphase.cpp:115
RemovingOverlapCallback
Definition: btSimpleBroadphase.cpp:95
RemovingOverlapCallback::processOverlap
virtual bool processOverlap(btBroadphasePair &pair)
Definition: btSimpleBroadphase.cpp:97
btAlignedObjectArray< btBroadphasePair >
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::quickSort
void quickSort(const L &CompareFunc)
Definition: btAlignedObjectArray.h:341
btBroadphasePairSortPredicate
Definition: btBroadphaseProxy.h:228
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btHashedOverlappingPairCache
Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman,...
Definition: btOverlappingPairCache.h:89
btOverlappingPairCache
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
Definition: btOverlappingPairCache.h:51
btOverlappingPairCache::getOverlappingPairArray
virtual btBroadphasePairArray & getOverlappingPairArray()=0
btOverlappingPairCache::findPair
virtual btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
btOverlappingPairCache::cleanOverlappingPair
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
btOverlappingPairCache::~btOverlappingPairCache
virtual ~btOverlappingPairCache()
Definition: btOverlappingPairCache.h:53
btOverlappingPairCache::hasDeferredRemoval
virtual bool hasDeferredRemoval()=0
btOverlappingPairCallback::removeOverlappingPair
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
btOverlappingPairCallback::removeOverlappingPairsContainingProxy
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
btOverlappingPairCallback::addOverlappingPair
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
btSimpleBroadphase::aabbTest
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
Definition: btSimpleBroadphase.cpp:161
btSimpleBroadphase::rayTest
virtual void rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, btBroadphaseRayCallback &rayCallback, const btVector3 &aabbMin=btVector3(0, 0, 0), const btVector3 &aabbMax=btVector3(0, 0, 0))
Definition: btSimpleBroadphase.cpp:148
btSimpleBroadphase::testAabbOverlap
bool testAabbOverlap(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
Definition: btSimpleBroadphase.cpp:315
btSimpleBroadphase::m_ownsPairCache
bool m_ownsPairCache
Definition: btSimpleBroadphase.h:83
btSimpleBroadphase::freeHandle
void freeHandle(btSimpleBroadphaseProxy *proxy)
Definition: btSimpleBroadphase.h:66
btSimpleBroadphase::m_maxHandles
int m_maxHandles
Definition: btSimpleBroadphase.h:45
btSimpleBroadphase::m_pairCache
btOverlappingPairCache * m_pairCache
Definition: btSimpleBroadphase.h:82
btSimpleBroadphase::resetPool
virtual void resetPool(btDispatcher *dispatcher)
reset broadphase internal structures, to ensure determinism/reproducability
Definition: btSimpleBroadphase.cpp:322
btSimpleBroadphase::setAabb
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
Definition: btSimpleBroadphase.cpp:141
btSimpleBroadphase::m_invalidPair
int m_invalidPair
Definition: btSimpleBroadphase.h:85
btSimpleBroadphase::m_firstFreeHandle
int m_firstFreeHandle
Definition: btSimpleBroadphase.h:51
btSimpleBroadphase::m_pHandlesRawPtr
void * m_pHandlesRawPtr
Definition: btSimpleBroadphase.h:50
btSimpleBroadphase::allocHandle
int allocHandle()
Definition: btSimpleBroadphase.h:53
btSimpleBroadphase::createProxy
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
Definition: btSimpleBroadphase.cpp:79
btSimpleBroadphase::m_pHandles
btSimpleBroadphaseProxy * m_pHandles
Definition: btSimpleBroadphase.h:48
btSimpleBroadphase::m_LastHandleIndex
int m_LastHandleIndex
Definition: btSimpleBroadphase.h:46
btSimpleBroadphase::btSimpleBroadphase
btSimpleBroadphase(int maxProxies=16384, btOverlappingPairCache *overlappingPairCache=0)
Definition: btSimpleBroadphase.cpp:38
btSimpleBroadphase::m_numHandles
int m_numHandles
Definition: btSimpleBroadphase.h:44
btSimpleBroadphase::getAabb
virtual void getAabb(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
Definition: btSimpleBroadphase.cpp:134
btSimpleBroadphase::destroyProxy
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
Definition: btSimpleBroadphase.cpp:124
btSimpleBroadphase::calculateOverlappingPairs
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
Definition: btSimpleBroadphase.cpp:194
btSimpleBroadphase::getSimpleProxyFromProxy
btSimpleBroadphaseProxy * getSimpleProxyFromProxy(btBroadphaseProxy *proxy)
Definition: btSimpleBroadphase.h:87
btSimpleBroadphase::aabbOverlap
static bool aabbOverlap(btSimpleBroadphaseProxy *proxy0, btSimpleBroadphaseProxy *proxy1)
Definition: btSimpleBroadphase.cpp:177
btSimpleBroadphase::~btSimpleBroadphase
virtual ~btSimpleBroadphase()
Definition: btSimpleBroadphase.cpp:68
btSimpleBroadphase::validate
void validate()
Definition: btSimpleBroadphase.cpp:27
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btBroadphaseAabbCallback
Definition: btBroadphaseInterface.h:26
btBroadphaseAabbCallback::process
virtual bool process(const btBroadphaseProxy *proxy)=0
btBroadphasePair
The btBroadphasePair class contains a pair of aabb-overlapping objects.
Definition: btBroadphaseProxy.h:179
btBroadphasePair::m_pProxy1
btBroadphaseProxy * m_pProxy1
Definition: btBroadphaseProxy.h:209
btBroadphasePair::m_pProxy0
btBroadphaseProxy * m_pProxy0
Definition: btBroadphaseProxy.h:208
btBroadphasePair::m_algorithm
btCollisionAlgorithm * m_algorithm
Definition: btBroadphaseProxy.h:211
btBroadphaseProxy
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
Definition: btBroadphaseProxy.h:86
btBroadphaseProxy::m_aabbMin
btVector3 m_aabbMin
Definition: btBroadphaseProxy.h:108
btBroadphaseProxy::m_clientObject
void * m_clientObject
Definition: btBroadphaseProxy.h:102
btBroadphaseProxy::m_aabbMax
btVector3 m_aabbMax
Definition: btBroadphaseProxy.h:109
btBroadphaseProxy::m_uniqueId
int m_uniqueId
Definition: btBroadphaseProxy.h:106
btBroadphaseRayCallback
Definition: btBroadphaseInterface.h:32
btOverlapCallback
Definition: btOverlappingPairCache.h:29
btSimpleBroadphaseProxy
Definition: btSimpleBroadphase.h:22
btSimpleBroadphaseProxy::SetNextFree
void SetNextFree(int next)
Definition: btSimpleBroadphase.h:35