Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h @ 12175

Last change on this file since 12175 was 12175, checked in by siramesh, 5 years ago

Super Orxo Bros (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 31.6 KB
Line 
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.
35template <typename BP_FP_INT_TYPE>
36class btAxisSweep3Internal : public btBroadphaseInterface
37{
38protected:
39
40        BP_FP_INT_TYPE  m_bpHandleMask;
41        BP_FP_INT_TYPE  m_handleSentinel;
42
43public:
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
56public:
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       
73protected:
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
124public:
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
208template <typename BP_FP_INT_TYPE>
209void 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
230template <typename BP_FP_INT_TYPE>
231btBroadphaseProxy*      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
248template <typename BP_FP_INT_TYPE>
249void    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
257template <typename BP_FP_INT_TYPE>
258void    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
269template <typename BP_FP_INT_TYPE>
270void    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
290template <typename BP_FP_INT_TYPE>
291void    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
317template <typename BP_FP_INT_TYPE>
318void 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
326template <typename BP_FP_INT_TYPE>
327void 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
351template <typename BP_FP_INT_TYPE>
352btAxisSweep3Internal<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),
354m_handleSentinel(handleSentinel),
355m_pairCache(pairCache),
356m_userPairCallback(0),
357m_ownsPairCache(false),
358m_invalidPair(0),
359m_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
434template <typename BP_FP_INT_TYPE>
435btAxisSweep3Internal<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
458template <typename BP_FP_INT_TYPE>
459void 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
480template <typename BP_FP_INT_TYPE>
481BP_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
492template <typename BP_FP_INT_TYPE>
493void 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
504template <typename BP_FP_INT_TYPE>
505BP_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
560template <typename BP_FP_INT_TYPE>
561void 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
617template <typename BP_FP_INT_TYPE>
618void 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
632extern int gOverlappingPairs;
633//#include <stdio.h>
634
635template <typename BP_FP_INT_TYPE>
636void    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
721template <typename BP_FP_INT_TYPE>
722bool 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
740template <typename BP_FP_INT_TYPE>
741bool 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
755template <typename BP_FP_INT_TYPE>
756void 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
806template <typename BP_FP_INT_TYPE>
807void 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
858template <typename BP_FP_INT_TYPE>
859void 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
914template <typename BP_FP_INT_TYPE>
915void 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
977template <typename BP_FP_INT_TYPE>
978void 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.
1031class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
1032{
1033public:
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.
1042class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
1043{
1044public:
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
Note: See TracBrowser for help on using the repository browser.