4f4d94b3cc7a03b4c24f80ca1e81d91014c6b36e
[blender.git] / extern / bullet2 / src / BulletCollision / BroadphaseCollision / btAxisSweep3.h
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
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         
68                 SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
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
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);
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
162         btOverlappingPairCache* getOverlappingPairCache()
163         {
164                 return m_pairCache;
165         }
166         const btOverlappingPairCache*   getOverlappingPairCache() const
167         {
168                 return m_pairCache;
169         }
170
171         void    setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
172         {
173                 m_userPairCallback = pairCallback;
174         }
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
182         virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
183         {
184                 aabbMin = m_worldAabbMin;
185                 aabbMax = m_worldAabbMax;
186         }
187
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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