Blender  V3.3
btSimpleBroadphase.cpp
Go to the documentation of this file.
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 "btSimpleBroadphase.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 
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 
38 btSimpleBroadphase::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);
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);
53  m_maxHandles = maxProxies;
54  m_numHandles = 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 
69 {
71 
72  if (m_ownsPairCache)
73  {
76  }
77 }
78 
79 btBroadphaseProxy* btSimpleBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
80 {
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 
95 {
96 protected:
97  virtual bool processOverlap(btBroadphasePair& pair)
98  {
99  (void)pair;
100  btAssert(0);
101  return false;
102  }
103 };
104 
106 {
107  btBroadphaseProxy* m_targetProxy;
108 
109 public:
111  {
112  }
113 
114 protected:
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 
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 {
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 {
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 
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 
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
186 {
187 public:
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 
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 
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  {
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 
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 
316 {
319  return aabbOverlap(p0, p1);
320 }
321 
323 {
324  //not yet
325 }
SIMD_FORCE_INLINE 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
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
btBroadphasePair
btBroadphaseProxy
btHashedOverlappingPairCache()
#define btAssert(x)
Definition: btScalar.h:295
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
virtual bool processOverlap(btBroadphasePair &pair)
virtual bool processOverlap(btBroadphasePair &pair)
virtual bool processOverlap(btBroadphasePair &pair)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
virtual btBroadphasePair * findPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
virtual bool hasDeferredRemoval()=0
virtual btBroadphasePairArray & getOverlappingPairArray()=0
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
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))
btSimpleBroadphaseProxy * getSimpleProxyFromProxy(btBroadphaseProxy *proxy)
bool testAabbOverlap(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
void freeHandle(btSimpleBroadphaseProxy *proxy)
btOverlappingPairCache * m_pairCache
virtual void resetPool(btDispatcher *dispatcher)
reset broadphase internal structures, to ensure determinism/reproducability
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
btSimpleBroadphaseProxy * m_pHandles
btSimpleBroadphase(int maxProxies=16384, btOverlappingPairCache *overlappingPairCache=0)
virtual void getAabb(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
static bool aabbOverlap(btSimpleBroadphaseProxy *proxy0, btSimpleBroadphaseProxy *proxy1)
DEGForeachIDComponentCallback callback
SyclQueue void void size_t num_bytes void
virtual bool process(const btBroadphaseProxy *proxy)=0
SIMD_FORCE_INLINE void SetNextFree(int next)