Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h @ 2760

Last change on this file since 2760 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

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