1 //Bullet Continuous Collision Detection and Physics Library
2 //Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
3
4 //
5 // btAxisSweep3.h
6 //
7 // Copyright (c) 2006 Simon Hobbs
8 //
9 // This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
10 //
11 // Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
12 //
13 // 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.
14 //
15 // 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
16 //
17 // 3. This notice may not be removed or altered from any source distribution.
18
19 #ifndef BT_AXIS_SWEEP_3_INTERNAL_H
20 #define BT_AXIS_SWEEP_3_INTERNAL_H
21
22 #include "LinearMath/btVector3.h"
23 #include "btOverlappingPairCache.h"
24 #include "btBroadphaseInterface.h"
25 #include "btBroadphaseProxy.h"
26 #include "btOverlappingPairCallback.h"
27 #include "btDbvtBroadphase.h"
28
29 //#define DEBUG_BROADPHASE 1
30 #define USE_OVERLAP_TEST_ON_REMOVES 1
31
32 /// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
33 /// It uses quantized integers to represent the begin and end points for each of the 3 axis.
34 /// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
35 template <typename BP_FP_INT_TYPE>
36 class btAxisSweep3Internal : public btBroadphaseInterface
37 {
38 protected:
39 BP_FP_INT_TYPE m_bpHandleMask;
40 BP_FP_INT_TYPE m_handleSentinel;
41
42 public:
43 BT_DECLARE_ALIGNED_ALLOCATOR();
44
45 class Edge
46 {
47 public:
48 BP_FP_INT_TYPE m_pos; // low bit is min/max
49 BP_FP_INT_TYPE m_handle;
50
IsMax()51 BP_FP_INT_TYPE IsMax() const { return static_cast<BP_FP_INT_TYPE>(m_pos & 1); }
52 };
53
54 public:
55 class Handle : public btBroadphaseProxy
56 {
57 public:
58 BT_DECLARE_ALIGNED_ALLOCATOR();
59
60 // indexes into the edge arrays
61 BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12
62 // BP_FP_INT_TYPE m_uniqueId;
63 btBroadphaseProxy* m_dbvtProxy; //for faster raycast
64 //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
65
SetNextFree(BP_FP_INT_TYPE next)66 SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) { m_minEdges[0] = next; }
GetNextFree()67 SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const { return m_minEdges[0]; }
68 }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry
69
70 protected:
71 btVector3 m_worldAabbMin; // overall system bounds
72 btVector3 m_worldAabbMax; // overall system bounds
73
74 btVector3 m_quantize; // scaling factor for quantization
75
76 BP_FP_INT_TYPE m_numHandles; // number of active handles
77 BP_FP_INT_TYPE m_maxHandles; // max number of handles
78 Handle* m_pHandles; // handles pool
79
80 BP_FP_INT_TYPE m_firstFreeHandle; // free handles list
81
82 Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
83 void* m_pEdgesRawPtr[3];
84
85 btOverlappingPairCache* m_pairCache;
86
87 ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
88 btOverlappingPairCallback* m_userPairCallback;
89
90 bool m_ownsPairCache;
91
92 int m_invalidPair;
93
94 ///additional dynamic aabb structure, used to accelerate ray cast queries.
95 ///can be disabled using a optional argument in the constructor
96 btDbvtBroadphase* m_raycastAccelerator;
97 btOverlappingPairCache* m_nullPairCache;
98
99 // allocation/deallocation
100 BP_FP_INT_TYPE allocHandle();
101 void freeHandle(BP_FP_INT_TYPE handle);
102
103 bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1);
104
105 #ifdef DEBUG_BROADPHASE
106 void debugPrintAxis(int axis, bool checkCardinality = true);
107 #endif //DEBUG_BROADPHASE
108
109 //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
110 //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
111
112 void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
113 void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
114 void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
115 void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
116
117 public:
118 btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
119
120 virtual ~btAxisSweep3Internal();
121
getNumHandles()122 BP_FP_INT_TYPE getNumHandles() const
123 {
124 return m_numHandles;
125 }
126
127 virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
128
129 BP_FP_INT_TYPE addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
130 void removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher);
131 void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
getHandle(BP_FP_INT_TYPE index)132 SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const { return m_pHandles + index; }
133
134 virtual void resetPool(btDispatcher* dispatcher);
135
136 void processAllOverlappingPairs(btOverlapCallback* callback);
137
138 //Broadphase Interface
139 virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
140 virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
141 virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
142 virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
143
144 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));
145 virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
146
147 void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
148 ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
149 void unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
150
151 bool testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
152
getOverlappingPairCache()153 btOverlappingPairCache* getOverlappingPairCache()
154 {
155 return m_pairCache;
156 }
getOverlappingPairCache()157 const btOverlappingPairCache* getOverlappingPairCache() const
158 {
159 return m_pairCache;
160 }
161
setOverlappingPairUserCallback(btOverlappingPairCallback * pairCallback)162 void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
163 {
164 m_userPairCallback = pairCallback;
165 }
getOverlappingPairUserCallback()166 const btOverlappingPairCallback* getOverlappingPairUserCallback() const
167 {
168 return m_userPairCallback;
169 }
170
171 ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
172 ///will add some transform later
getBroadphaseAabb(btVector3 & aabbMin,btVector3 & aabbMax)173 virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
174 {
175 aabbMin = m_worldAabbMin;
176 aabbMax = m_worldAabbMax;
177 }
178
printStats()179 virtual void printStats()
180 {
181 /* printf("btAxisSweep3.h\n");
182 printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
183 printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
184 m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
185 */
186 }
187 };
188
189 ////////////////////////////////////////////////////////////////////
190
191 #ifdef DEBUG_BROADPHASE
192 #include <stdio.h>
193
194 template <typename BP_FP_INT_TYPE>
debugPrintAxis(int axis,bool checkCardinality)195 void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
196 {
197 int numEdges = m_pHandles[0].m_maxEdges[axis];
198 printf("SAP Axis %d, numEdges=%d\n", axis, numEdges);
199
200 int i;
201 for (i = 0; i < numEdges + 1; i++)
202 {
203 Edge* pEdge = m_pEdges[axis] + i;
204 Handle* pHandlePrev = getHandle(pEdge->m_handle);
205 int handleIndex = pEdge->IsMax() ? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
206 char beginOrEnd;
207 beginOrEnd = pEdge->IsMax() ? 'E' : 'B';
208 printf(" [%c,h=%d,p=%x,i=%d]\n", beginOrEnd, pEdge->m_handle, pEdge->m_pos, handleIndex);
209 }
210
211 if (checkCardinality)
212 btAssert(numEdges == m_numHandles * 2 + 1);
213 }
214 #endif //DEBUG_BROADPHASE
215
216 template <typename BP_FP_INT_TYPE>
createProxy(const btVector3 & aabbMin,const btVector3 & aabbMax,int shapeType,void * userPtr,int collisionFilterGroup,int collisionFilterMask,btDispatcher * dispatcher)217 btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher)
218 {
219 (void)shapeType;
220 BP_FP_INT_TYPE handleId = addHandle(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
221
222 Handle* handle = getHandle(handleId);
223
224 if (m_raycastAccelerator)
225 {
226 btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
227 handle->m_dbvtProxy = rayProxy;
228 }
229 return handle;
230 }
231
232 template <typename BP_FP_INT_TYPE>
destroyProxy(btBroadphaseProxy * proxy,btDispatcher * dispatcher)233 void btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
234 {
235 Handle* handle = static_cast<Handle*>(proxy);
236 if (m_raycastAccelerator)
237 m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy, dispatcher);
238 removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
239 }
240
241 template <typename BP_FP_INT_TYPE>
setAabb(btBroadphaseProxy * proxy,const btVector3 & aabbMin,const btVector3 & aabbMax,btDispatcher * dispatcher)242 void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher)
243 {
244 Handle* handle = static_cast<Handle*>(proxy);
245 handle->m_aabbMin = aabbMin;
246 handle->m_aabbMax = aabbMax;
247 updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax, dispatcher);
248 if (m_raycastAccelerator)
249 m_raycastAccelerator->setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher);
250 }
251
252 template <typename BP_FP_INT_TYPE>
rayTest(const btVector3 & rayFrom,const btVector3 & rayTo,btBroadphaseRayCallback & rayCallback,const btVector3 & aabbMin,const btVector3 & aabbMax)253 void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
254 {
255 if (m_raycastAccelerator)
256 {
257 m_raycastAccelerator->rayTest(rayFrom, rayTo, rayCallback, aabbMin, aabbMax);
258 }
259 else
260 {
261 //choose axis?
262 BP_FP_INT_TYPE axis = 0;
263 //for each proxy
264 for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
265 {
266 if (m_pEdges[axis][i].IsMax())
267 {
268 rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
269 }
270 }
271 }
272 }
273
274 template <typename BP_FP_INT_TYPE>
aabbTest(const btVector3 & aabbMin,const btVector3 & aabbMax,btBroadphaseAabbCallback & callback)275 void btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
276 {
277 if (m_raycastAccelerator)
278 {
279 m_raycastAccelerator->aabbTest(aabbMin, aabbMax, callback);
280 }
281 else
282 {
283 //choose axis?
284 BP_FP_INT_TYPE axis = 0;
285 //for each proxy
286 for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
287 {
288 if (m_pEdges[axis][i].IsMax())
289 {
290 Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
291 if (TestAabbAgainstAabb2(aabbMin, aabbMax, handle->m_aabbMin, handle->m_aabbMax))
292 {
293 callback.process(handle);
294 }
295 }
296 }
297 }
298 }
299
300 template <typename BP_FP_INT_TYPE>
getAabb(btBroadphaseProxy * proxy,btVector3 & aabbMin,btVector3 & aabbMax)301 void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
302 {
303 Handle* pHandle = static_cast<Handle*>(proxy);
304 aabbMin = pHandle->m_aabbMin;
305 aabbMax = pHandle->m_aabbMax;
306 }
307
308 template <typename BP_FP_INT_TYPE>
unQuantize(btBroadphaseProxy * proxy,btVector3 & aabbMin,btVector3 & aabbMax)309 void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
310 {
311 Handle* pHandle = static_cast<Handle*>(proxy);
312
313 unsigned short vecInMin[3];
314 unsigned short vecInMax[3];
315
316 vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos;
317 vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos + 1;
318 vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos;
319 vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos + 1;
320 vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos;
321 vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos + 1;
322
323 aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()), (btScalar)(vecInMin[1]) / (m_quantize.getY()), (btScalar)(vecInMin[2]) / (m_quantize.getZ()));
324 aabbMin += m_worldAabbMin;
325
326 aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()), (btScalar)(vecInMax[1]) / (m_quantize.getY()), (btScalar)(vecInMax[2]) / (m_quantize.getZ()));
327 aabbMax += m_worldAabbMin;
328 }
329
330 template <typename BP_FP_INT_TYPE>
btAxisSweep3Internal(const btVector3 & worldAabbMin,const btVector3 & worldAabbMax,BP_FP_INT_TYPE handleMask,BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles,btOverlappingPairCache * pairCache,bool disableRaycastAccelerator)331 btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
332 : m_bpHandleMask(handleMask),
333 m_handleSentinel(handleSentinel),
334 m_pairCache(pairCache),
335 m_userPairCallback(0),
336 m_ownsPairCache(false),
337 m_invalidPair(0),
338 m_raycastAccelerator(0)
339 {
340 BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles + 1); //need to add one sentinel handle
341
342 if (!m_pairCache)
343 {
344 void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
345 m_pairCache = new (ptr) btHashedOverlappingPairCache();
346 m_ownsPairCache = true;
347 }
348
349 if (!disableRaycastAccelerator)
350 {
351 m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache), 16)) btNullPairCache();
352 m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase), 16)) btDbvtBroadphase(m_nullPairCache); //m_pairCache);
353 m_raycastAccelerator->m_deferedcollide = true; //don't add/remove pairs
354 }
355
356 //btAssert(bounds.HasVolume());
357
358 // init bounds
359 m_worldAabbMin = worldAabbMin;
360 m_worldAabbMax = worldAabbMax;
361
362 btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
363
364 BP_FP_INT_TYPE maxInt = m_handleSentinel;
365
366 m_quantize = btVector3(btScalar(maxInt), btScalar(maxInt), btScalar(maxInt)) / aabbSize;
367
368 // allocate handles buffer, using btAlignedAlloc, and put all handles on free list
369 m_pHandles = new Handle[maxHandles];
370
371 m_maxHandles = maxHandles;
372 m_numHandles = 0;
373
374 // handle 0 is reserved as the null index, and is also used as the sentinel
375 m_firstFreeHandle = 1;
376 {
377 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
378 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
379 m_pHandles[maxHandles - 1].SetNextFree(0);
380 }
381
382 {
383 // allocate edge buffers
384 for (int i = 0; i < 3; i++)
385 {
386 m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge) * maxHandles * 2, 16);
387 m_pEdges[i] = new (m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
388 }
389 }
390 //removed overlap management
391
392 // make boundary sentinels
393
394 m_pHandles[0].m_clientObject = 0;
395
396 for (int axis = 0; axis < 3; axis++)
397 {
398 m_pHandles[0].m_minEdges[axis] = 0;
399 m_pHandles[0].m_maxEdges[axis] = 1;
400
401 m_pEdges[axis][0].m_pos = 0;
402 m_pEdges[axis][0].m_handle = 0;
403 m_pEdges[axis][1].m_pos = m_handleSentinel;
404 m_pEdges[axis][1].m_handle = 0;
405 #ifdef DEBUG_BROADPHASE
406 debugPrintAxis(axis);
407 #endif //DEBUG_BROADPHASE
408 }
409 }
410
411 template <typename BP_FP_INT_TYPE>
~btAxisSweep3Internal()412 btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
413 {
414 if (m_raycastAccelerator)
415 {
416 m_nullPairCache->~btOverlappingPairCache();
417 btAlignedFree(m_nullPairCache);
418 m_raycastAccelerator->~btDbvtBroadphase();
419 btAlignedFree(m_raycastAccelerator);
420 }
421
422 for (int i = 2; i >= 0; i--)
423 {
424 btAlignedFree(m_pEdgesRawPtr[i]);
425 }
426 delete[] m_pHandles;
427
428 if (m_ownsPairCache)
429 {
430 m_pairCache->~btOverlappingPairCache();
431 btAlignedFree(m_pairCache);
432 }
433 }
434
435 template <typename BP_FP_INT_TYPE>
quantize(BP_FP_INT_TYPE * out,const btVector3 & point,int isMax)436 void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
437 {
438 #ifdef OLD_CLAMPING_METHOD
439 ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax]
440 ///see http://code.google.com/p/bullet/issues/detail?id=87
441 btVector3 clampedPoint(point);
442 clampedPoint.setMax(m_worldAabbMin);
443 clampedPoint.setMin(m_worldAabbMax);
444 btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
445 out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
446 out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
447 out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
448 #else
449 btVector3 v = (point - m_worldAabbMin) * m_quantize;
450 out[0] = (v[0] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[0] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0] & m_bpHandleMask) | isMax);
451 out[1] = (v[1] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[1] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1] & m_bpHandleMask) | isMax);
452 out[2] = (v[2] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[2] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2] & m_bpHandleMask) | isMax);
453 #endif //OLD_CLAMPING_METHOD
454 }
455
456 template <typename BP_FP_INT_TYPE>
allocHandle()457 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
458 {
459 btAssert(m_firstFreeHandle);
460
461 BP_FP_INT_TYPE handle = m_firstFreeHandle;
462 m_firstFreeHandle = getHandle(handle)->GetNextFree();
463 m_numHandles++;
464
465 return handle;
466 }
467
468 template <typename BP_FP_INT_TYPE>
freeHandle(BP_FP_INT_TYPE handle)469 void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
470 {
471 btAssert(handle > 0 && handle < m_maxHandles);
472
473 getHandle(handle)->SetNextFree(m_firstFreeHandle);
474 m_firstFreeHandle = handle;
475
476 m_numHandles--;
477 }
478
479 template <typename BP_FP_INT_TYPE>
addHandle(const btVector3 & aabbMin,const btVector3 & aabbMax,void * pOwner,int collisionFilterGroup,int collisionFilterMask,btDispatcher * dispatcher)480 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher)
481 {
482 // quantize the bounds
483 BP_FP_INT_TYPE min[3], max[3];
484 quantize(min, aabbMin, 0);
485 quantize(max, aabbMax, 1);
486
487 // allocate a handle
488 BP_FP_INT_TYPE handle = allocHandle();
489
490 Handle* pHandle = getHandle(handle);
491
492 pHandle->m_uniqueId = static_cast<int>(handle);
493 //pHandle->m_pOverlaps = 0;
494 pHandle->m_clientObject = pOwner;
495 pHandle->m_collisionFilterGroup = collisionFilterGroup;
496 pHandle->m_collisionFilterMask = collisionFilterMask;
497
498 // compute current limit of edge arrays
499 BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
500
501 // insert new edges just inside the max boundary edge
502 for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
503 {
504 m_pHandles[0].m_maxEdges[axis] += 2;
505
506 m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
507
508 m_pEdges[axis][limit - 1].m_pos = min[axis];
509 m_pEdges[axis][limit - 1].m_handle = handle;
510
511 m_pEdges[axis][limit].m_pos = max[axis];
512 m_pEdges[axis][limit].m_handle = handle;
513
514 pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
515 pHandle->m_maxEdges[axis] = limit;
516 }
517
518 // now sort the new edges to their correct position
519 sortMinDown(0, pHandle->m_minEdges[0], dispatcher, false);
520 sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher, false);
521 sortMinDown(1, pHandle->m_minEdges[1], dispatcher, false);
522 sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher, false);
523 sortMinDown(2, pHandle->m_minEdges[2], dispatcher, true);
524 sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher, true);
525
526 return handle;
527 }
528
529 template <typename BP_FP_INT_TYPE>
removeHandle(BP_FP_INT_TYPE handle,btDispatcher * dispatcher)530 void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher)
531 {
532 Handle* pHandle = getHandle(handle);
533
534 //explicitly remove the pairs containing the proxy
535 //we could do it also in the sortMinUp (passing true)
536 ///@todo: compare performance
537 if (!m_pairCache->hasDeferredRemoval())
538 {
539 m_pairCache->removeOverlappingPairsContainingProxy(pHandle, dispatcher);
540 }
541
542 // compute current limit of edge arrays
543 int limit = static_cast<int>(m_numHandles * 2);
544
545 int axis;
546
547 for (axis = 0; axis < 3; axis++)
548 {
549 m_pHandles[0].m_maxEdges[axis] -= 2;
550 }
551
552 // remove the edges by sorting them up to the end of the list
553 for (axis = 0; axis < 3; axis++)
554 {
555 Edge* pEdges = m_pEdges[axis];
556 BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
557 pEdges[max].m_pos = m_handleSentinel;
558
559 sortMaxUp(axis, max, dispatcher, false);
560
561 BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
562 pEdges[i].m_pos = m_handleSentinel;
563
564 sortMinUp(axis, i, dispatcher, false);
565
566 pEdges[limit - 1].m_handle = 0;
567 pEdges[limit - 1].m_pos = m_handleSentinel;
568
569 #ifdef DEBUG_BROADPHASE
570 debugPrintAxis(axis, false);
571 #endif //DEBUG_BROADPHASE
572 }
573
574 // free the handle
575 freeHandle(handle);
576 }
577
578 template <typename BP_FP_INT_TYPE>
resetPool(btDispatcher *)579 void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/)
580 {
581 if (m_numHandles == 0)
582 {
583 m_firstFreeHandle = 1;
584 {
585 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
586 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
587 m_pHandles[m_maxHandles - 1].SetNextFree(0);
588 }
589 }
590 }
591
592 //#include <stdio.h>
593
594 template <typename BP_FP_INT_TYPE>
calculateOverlappingPairs(btDispatcher * dispatcher)595 void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
596 {
597 if (m_pairCache->hasDeferredRemoval())
598 {
599 btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
600
601 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
602 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
603
604 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
605 m_invalidPair = 0;
606
607 int i;
608
609 btBroadphasePair previousPair;
610 previousPair.m_pProxy0 = 0;
611 previousPair.m_pProxy1 = 0;
612 previousPair.m_algorithm = 0;
613
614 for (i = 0; i < overlappingPairArray.size(); i++)
615 {
616 btBroadphasePair& pair = overlappingPairArray[i];
617
618 bool isDuplicate = (pair == previousPair);
619
620 previousPair = pair;
621
622 bool needsRemoval = false;
623
624 if (!isDuplicate)
625 {
626 ///important to use an AABB test that is consistent with the broadphase
627 bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
628
629 if (hasOverlap)
630 {
631 needsRemoval = false; //callback->processOverlap(pair);
632 }
633 else
634 {
635 needsRemoval = true;
636 }
637 }
638 else
639 {
640 //remove duplicate
641 needsRemoval = true;
642 //should have no algorithm
643 btAssert(!pair.m_algorithm);
644 }
645
646 if (needsRemoval)
647 {
648 m_pairCache->cleanOverlappingPair(pair, dispatcher);
649
650 // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
651 // m_overlappingPairArray.pop_back();
652 pair.m_pProxy0 = 0;
653 pair.m_pProxy1 = 0;
654 m_invalidPair++;
655 }
656 }
657
658 ///if you don't like to skip the invalid pairs in the array, execute following code:
659 #define CLEAN_INVALID_PAIRS 1
660 #ifdef CLEAN_INVALID_PAIRS
661
662 //perform a sort, to sort 'invalid' pairs to the end
663 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
664
665 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
666 m_invalidPair = 0;
667 #endif //CLEAN_INVALID_PAIRS
668
669 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
670 }
671 }
672
673 template <typename BP_FP_INT_TYPE>
testAabbOverlap(btBroadphaseProxy * proxy0,btBroadphaseProxy * proxy1)674 bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
675 {
676 const Handle* pHandleA = static_cast<Handle*>(proxy0);
677 const Handle* pHandleB = static_cast<Handle*>(proxy1);
678
679 //optimization 1: check the array index (memory address), instead of the m_pos
680
681 for (int axis = 0; axis < 3; axis++)
682 {
683 if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
684 pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
685 {
686 return false;
687 }
688 }
689 return true;
690 }
691
692 template <typename BP_FP_INT_TYPE>
testOverlap2D(const Handle * pHandleA,const Handle * pHandleB,int axis0,int axis1)693 bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1)
694 {
695 //optimization 1: check the array index (memory address), instead of the m_pos
696
697 if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] ||
698 pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
699 pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
700 pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1])
701 {
702 return false;
703 }
704 return true;
705 }
706
707 template <typename BP_FP_INT_TYPE>
updateHandle(BP_FP_INT_TYPE handle,const btVector3 & aabbMin,const btVector3 & aabbMax,btDispatcher * dispatcher)708 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher)
709 {
710 // btAssert(bounds.IsFinite());
711 //btAssert(bounds.HasVolume());
712
713 Handle* pHandle = getHandle(handle);
714
715 // quantize the new bounds
716 BP_FP_INT_TYPE min[3], max[3];
717 quantize(min, aabbMin, 0);
718 quantize(max, aabbMax, 1);
719
720 // update changed edges
721 for (int axis = 0; axis < 3; axis++)
722 {
723 BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
724 BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
725
726 int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
727 int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
728
729 m_pEdges[axis][emin].m_pos = min[axis];
730 m_pEdges[axis][emax].m_pos = max[axis];
731
732 // expand (only adds overlaps)
733 if (dmin < 0)
734 sortMinDown(axis, emin, dispatcher, true);
735
736 if (dmax > 0)
737 sortMaxUp(axis, emax, dispatcher, true);
738
739 // shrink (only removes overlaps)
740 if (dmin > 0)
741 sortMinUp(axis, emin, dispatcher, true);
742
743 if (dmax < 0)
744 sortMaxDown(axis, emax, dispatcher, true);
745
746 #ifdef DEBUG_BROADPHASE
747 debugPrintAxis(axis);
748 #endif //DEBUG_BROADPHASE
749 }
750 }
751
752 // sorting a min edge downwards can only ever *add* overlaps
753 template <typename BP_FP_INT_TYPE>
sortMinDown(int axis,BP_FP_INT_TYPE edge,btDispatcher *,bool updateOverlaps)754 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
755 {
756 Edge* pEdge = m_pEdges[axis] + edge;
757 Edge* pPrev = pEdge - 1;
758 Handle* pHandleEdge = getHandle(pEdge->m_handle);
759
760 while (pEdge->m_pos < pPrev->m_pos)
761 {
762 Handle* pHandlePrev = getHandle(pPrev->m_handle);
763
764 if (pPrev->IsMax())
765 {
766 // if previous edge is a maximum check the bounds and add an overlap if necessary
767 const int axis1 = (1 << axis) & 3;
768 const int axis2 = (1 << axis1) & 3;
769 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2))
770 {
771 m_pairCache->addOverlappingPair(pHandleEdge, pHandlePrev);
772 if (m_userPairCallback)
773 m_userPairCallback->addOverlappingPair(pHandleEdge, pHandlePrev);
774
775 //AddOverlap(pEdge->m_handle, pPrev->m_handle);
776 }
777
778 // update edge reference in other handle
779 pHandlePrev->m_maxEdges[axis]++;
780 }
781 else
782 pHandlePrev->m_minEdges[axis]++;
783
784 pHandleEdge->m_minEdges[axis]--;
785
786 // swap the edges
787 Edge swap = *pEdge;
788 *pEdge = *pPrev;
789 *pPrev = swap;
790
791 // decrement
792 pEdge--;
793 pPrev--;
794 }
795
796 #ifdef DEBUG_BROADPHASE
797 debugPrintAxis(axis);
798 #endif //DEBUG_BROADPHASE
799 }
800
801 // sorting a min edge upwards can only ever *remove* overlaps
802 template <typename BP_FP_INT_TYPE>
sortMinUp(int axis,BP_FP_INT_TYPE edge,btDispatcher * dispatcher,bool updateOverlaps)803 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
804 {
805 Edge* pEdge = m_pEdges[axis] + edge;
806 Edge* pNext = pEdge + 1;
807 Handle* pHandleEdge = getHandle(pEdge->m_handle);
808
809 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
810 {
811 Handle* pHandleNext = getHandle(pNext->m_handle);
812
813 if (pNext->IsMax())
814 {
815 Handle* handle0 = getHandle(pEdge->m_handle);
816 Handle* handle1 = getHandle(pNext->m_handle);
817 const int axis1 = (1 << axis) & 3;
818 const int axis2 = (1 << axis1) & 3;
819
820 // if next edge is maximum remove any overlap between the two handles
821 if (updateOverlaps
822 #ifdef USE_OVERLAP_TEST_ON_REMOVES
823 && testOverlap2D(handle0, handle1, axis1, axis2)
824 #endif //USE_OVERLAP_TEST_ON_REMOVES
825 )
826 {
827 m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
828 if (m_userPairCallback)
829 m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher);
830 }
831
832 // update edge reference in other handle
833 pHandleNext->m_maxEdges[axis]--;
834 }
835 else
836 pHandleNext->m_minEdges[axis]--;
837
838 pHandleEdge->m_minEdges[axis]++;
839
840 // swap the edges
841 Edge swap = *pEdge;
842 *pEdge = *pNext;
843 *pNext = swap;
844
845 // increment
846 pEdge++;
847 pNext++;
848 }
849 }
850
851 // sorting a max edge downwards can only ever *remove* overlaps
852 template <typename BP_FP_INT_TYPE>
sortMaxDown(int axis,BP_FP_INT_TYPE edge,btDispatcher * dispatcher,bool updateOverlaps)853 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
854 {
855 Edge* pEdge = m_pEdges[axis] + edge;
856 Edge* pPrev = pEdge - 1;
857 Handle* pHandleEdge = getHandle(pEdge->m_handle);
858
859 while (pEdge->m_pos < pPrev->m_pos)
860 {
861 Handle* pHandlePrev = getHandle(pPrev->m_handle);
862
863 if (!pPrev->IsMax())
864 {
865 // if previous edge was a minimum remove any overlap between the two handles
866 Handle* handle0 = getHandle(pEdge->m_handle);
867 Handle* handle1 = getHandle(pPrev->m_handle);
868 const int axis1 = (1 << axis) & 3;
869 const int axis2 = (1 << axis1) & 3;
870
871 if (updateOverlaps
872 #ifdef USE_OVERLAP_TEST_ON_REMOVES
873 && testOverlap2D(handle0, handle1, axis1, axis2)
874 #endif //USE_OVERLAP_TEST_ON_REMOVES
875 )
876 {
877 //this is done during the overlappingpairarray iteration/narrowphase collision
878
879 m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
880 if (m_userPairCallback)
881 m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher);
882 }
883
884 // update edge reference in other handle
885 pHandlePrev->m_minEdges[axis]++;
886 ;
887 }
888 else
889 pHandlePrev->m_maxEdges[axis]++;
890
891 pHandleEdge->m_maxEdges[axis]--;
892
893 // swap the edges
894 Edge swap = *pEdge;
895 *pEdge = *pPrev;
896 *pPrev = swap;
897
898 // decrement
899 pEdge--;
900 pPrev--;
901 }
902
903 #ifdef DEBUG_BROADPHASE
904 debugPrintAxis(axis);
905 #endif //DEBUG_BROADPHASE
906 }
907
908 // sorting a max edge upwards can only ever *add* overlaps
909 template <typename BP_FP_INT_TYPE>
sortMaxUp(int axis,BP_FP_INT_TYPE edge,btDispatcher *,bool updateOverlaps)910 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
911 {
912 Edge* pEdge = m_pEdges[axis] + edge;
913 Edge* pNext = pEdge + 1;
914 Handle* pHandleEdge = getHandle(pEdge->m_handle);
915
916 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
917 {
918 Handle* pHandleNext = getHandle(pNext->m_handle);
919
920 const int axis1 = (1 << axis) & 3;
921 const int axis2 = (1 << axis1) & 3;
922
923 if (!pNext->IsMax())
924 {
925 // if next edge is a minimum check the bounds and add an overlap if necessary
926 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext, axis1, axis2))
927 {
928 Handle* handle0 = getHandle(pEdge->m_handle);
929 Handle* handle1 = getHandle(pNext->m_handle);
930 m_pairCache->addOverlappingPair(handle0, handle1);
931 if (m_userPairCallback)
932 m_userPairCallback->addOverlappingPair(handle0, handle1);
933 }
934
935 // update edge reference in other handle
936 pHandleNext->m_minEdges[axis]--;
937 }
938 else
939 pHandleNext->m_maxEdges[axis]--;
940
941 pHandleEdge->m_maxEdges[axis]++;
942
943 // swap the edges
944 Edge swap = *pEdge;
945 *pEdge = *pNext;
946 *pNext = swap;
947
948 // increment
949 pEdge++;
950 pNext++;
951 }
952 }
953
954 #endif
955