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