Back to index

supertuxkart  0.5+dfsg1
btAxisSweep3.h
Go to the documentation of this file.
00001 //Bullet Continuous Collision Detection and Physics Library
00002 //Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00003 
00004 //
00005 // btAxisSweep3.h
00006 //
00007 // Copyright (c) 2006 Simon Hobbs
00008 //
00009 // 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.
00010 //
00011 // 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:
00012 //
00013 // 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.
00014 //
00015 // 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00016 //
00017 // 3. This notice may not be removed or altered from any source distribution.
00018 
00019 #ifndef AXIS_SWEEP_3_H
00020 #define AXIS_SWEEP_3_H
00021 
00022 #include "LinearMath/btPoint3.h"
00023 #include "LinearMath/btVector3.h"
00024 #include "btOverlappingPairCache.h"
00025 #include "btBroadphaseInterface.h"
00026 #include "btBroadphaseProxy.h"
00027 #include "btOverlappingPairCallback.h"
00028 
00029 //#define DEBUG_BROADPHASE 1
00030 
00033 template <typename BP_FP_INT_TYPE>
00034 class btAxisSweep3Internal : public btBroadphaseInterface
00035 {
00036 protected:
00037 
00038        BP_FP_INT_TYPE       m_bpHandleMask;
00039        BP_FP_INT_TYPE       m_handleSentinel;
00040 
00041 public:
00042        
00043 
00044        class Edge
00045        {
00046        public:
00047               BP_FP_INT_TYPE m_pos;                     // low bit is min/max
00048               BP_FP_INT_TYPE m_handle;
00049 
00050               BP_FP_INT_TYPE IsMax() const {return m_pos & 1;}
00051        };
00052 
00053 public:
00054        ATTRIBUTE_ALIGNED16(class) Handle : public btBroadphaseProxy
00055        {
00056        public:
00057        BT_DECLARE_ALIGNED_ALLOCATOR();
00058        
00059               // indexes into the edge arrays
00060               BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];            // 6 * 2 = 12
00061 //            BP_FP_INT_TYPE m_uniqueId;
00062               BP_FP_INT_TYPE m_pad;
00063               
00064               //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
00065        
00066               SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
00067               SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
00068        };            // 24 bytes + 24 for Edge structures = 44 bytes total per entry
00069 
00070        
00071 protected:
00072        btPoint3 m_worldAabbMin;                                       // overall system bounds
00073        btPoint3 m_worldAabbMax;                                       // overall system bounds
00074 
00075        btVector3 m_quantize;                                          // scaling factor for quantization
00076 
00077        BP_FP_INT_TYPE m_numHandles;                                          // number of active handles
00078        BP_FP_INT_TYPE m_maxHandles;                                          // max number of handles
00079        Handle* m_pHandles;                                     // handles pool
00080        BP_FP_INT_TYPE m_firstFreeHandle;         // free handles list
00081 
00082        Edge* m_pEdges[3];                                      // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
00083 
00084        btOverlappingPairCache* m_pairCache;
00085 
00087        btOverlappingPairCallback* m_userPairCallback;
00088        
00089        bool   m_ownsPairCache;
00090 
00091        int    m_invalidPair;
00092 
00093        // allocation/deallocation
00094        BP_FP_INT_TYPE allocHandle();
00095        void freeHandle(BP_FP_INT_TYPE handle);
00096        
00097 
00098        bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB);
00099 
00100 #ifdef DEBUG_BROADPHASE
00101        void debugPrintAxis(int axis,bool checkCardinality=true);
00102 #endif //DEBUG_BROADPHASE
00103 
00104        //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
00105        //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
00106 
00107        void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
00108 
00109        void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
00110        void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
00111        void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
00112        void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
00113 
00114 public:
00115 
00116        btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0);
00117 
00118        virtual       ~btAxisSweep3Internal();
00119 
00120        BP_FP_INT_TYPE getNumHandles() const
00121        {
00122               return m_numHandles;
00123        }
00124 
00125        virtual void  calculateOverlappingPairs(btDispatcher* dispatcher);
00126        
00127        BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
00128        void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
00129        void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
00130        SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
00131 
00132        void   processAllOverlappingPairs(btOverlapCallback* callback);
00133 
00134        //Broadphase Interface
00135        virtual btBroadphaseProxy*  createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
00136        virtual void  destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
00137        virtual void  setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
00138        
00139        bool   testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
00140 
00141        btOverlappingPairCache*     getOverlappingPairCache()
00142        {
00143               return m_pairCache;
00144        }
00145        const btOverlappingPairCache*      getOverlappingPairCache() const
00146        {
00147               return m_pairCache;
00148        }
00149 
00150        void   setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
00151        {
00152               m_userPairCallback = pairCallback;
00153        }
00154        const btOverlappingPairCallback*   getOverlappingPairUserCallback() const
00155        {
00156               return m_userPairCallback;
00157        }
00158 
00161        virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
00162        {
00163               aabbMin = m_worldAabbMin;
00164               aabbMax = m_worldAabbMax;
00165        }
00166 
00167        virtual void  printStats()
00168        {
00169 /*            printf("btAxisSweep3.h\n");
00170               printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
00171               printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
00172                      m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
00173                      */
00174 
00175        }
00176 
00177 };
00178 
00180 
00181 
00182 
00183 
00184 #ifdef DEBUG_BROADPHASE
00185 #include <stdio.h>
00186 
00187 template <typename BP_FP_INT_TYPE>
00188 void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
00189 {
00190        int numEdges = m_pHandles[0].m_maxEdges[axis];
00191        printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
00192 
00193        int i;
00194        for (i=0;i<numEdges+1;i++)
00195        {
00196               Edge* pEdge = m_pEdges[axis] + i;
00197               Handle* pHandlePrev = getHandle(pEdge->m_handle);
00198               int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
00199               char beginOrEnd;
00200               beginOrEnd=pEdge->IsMax()?'E':'B';
00201               printf("      [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
00202        }
00203 
00204        if (checkCardinality)
00205               assert(numEdges == m_numHandles*2+1);
00206 }
00207 #endif //DEBUG_BROADPHASE
00208 
00209 template <typename BP_FP_INT_TYPE>
00210 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)
00211 {
00212               (void)shapeType;
00213               BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
00214               
00215               Handle* handle = getHandle(handleId);
00216                             
00217               return handle;
00218 }
00219 
00220 
00221 
00222 template <typename BP_FP_INT_TYPE>
00223 void   btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
00224 {
00225        Handle* handle = static_cast<Handle*>(proxy);
00226        removeHandle(handle->m_uniqueId,dispatcher);
00227 }
00228 
00229 template <typename BP_FP_INT_TYPE>
00230 void   btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
00231 {
00232        Handle* handle = static_cast<Handle*>(proxy);
00233        updateHandle(handle->m_uniqueId,aabbMin,aabbMax,dispatcher);
00234 
00235 }
00236 
00237 
00238 
00239 
00240 
00241 template <typename BP_FP_INT_TYPE>
00242 btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache )
00243 :m_bpHandleMask(handleMask),
00244 m_handleSentinel(handleSentinel),
00245 m_pairCache(pairCache),
00246 m_userPairCallback(0),
00247 m_ownsPairCache(false),
00248 m_invalidPair(0)
00249 {
00250        BP_FP_INT_TYPE maxHandles = userMaxHandles+1;//need to add one sentinel handle
00251 
00252        if (!m_pairCache)
00253        {
00254               void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
00255               m_pairCache = new(ptr) btHashedOverlappingPairCache();
00256               m_ownsPairCache = true;
00257        }
00258 
00259        //assert(bounds.HasVolume());
00260 
00261        // init bounds
00262        m_worldAabbMin = worldAabbMin;
00263        m_worldAabbMax = worldAabbMax;
00264 
00265        btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
00266 
00267        BP_FP_INT_TYPE       maxInt = m_handleSentinel;
00268 
00269        m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
00270 
00271        // allocate handles buffer and put all handles on free list
00272        void* ptr = btAlignedAlloc(sizeof(Handle)*maxHandles,16);
00273        m_pHandles = new(ptr) Handle[maxHandles];
00274        m_maxHandles = maxHandles;
00275        m_numHandles = 0;
00276 
00277        // handle 0 is reserved as the null index, and is also used as the sentinel
00278        m_firstFreeHandle = 1;
00279        {
00280               for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
00281                      m_pHandles[i].SetNextFree(i + 1);
00282               m_pHandles[maxHandles - 1].SetNextFree(0);
00283        }
00284 
00285        {
00286               // allocate edge buffers
00287               for (int i = 0; i < 3; i++)
00288               {
00289                      void* ptr = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
00290                      m_pEdges[i] = new(ptr) Edge[maxHandles * 2];
00291               }
00292        }
00293        //removed overlap management
00294 
00295        // make boundary sentinels
00296        
00297        m_pHandles[0].m_clientObject = 0;
00298 
00299        for (int axis = 0; axis < 3; axis++)
00300        {
00301               m_pHandles[0].m_minEdges[axis] = 0;
00302               m_pHandles[0].m_maxEdges[axis] = 1;
00303 
00304               m_pEdges[axis][0].m_pos = 0;
00305               m_pEdges[axis][0].m_handle = 0;
00306               m_pEdges[axis][1].m_pos = m_handleSentinel;
00307               m_pEdges[axis][1].m_handle = 0;
00308 #ifdef DEBUG_BROADPHASE
00309               debugPrintAxis(axis);
00310 #endif //DEBUG_BROADPHASE
00311 
00312        }
00313 
00314 }
00315 
00316 template <typename BP_FP_INT_TYPE>
00317 btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
00318 {
00319        
00320        for (int i = 2; i >= 0; i--)
00321        {
00322               btAlignedFree(m_pEdges[i]);
00323        }
00324        btAlignedFree(m_pHandles);
00325 
00326        if (m_ownsPairCache)
00327        {
00328               m_pairCache->~btOverlappingPairCache();
00329               btAlignedFree(m_pairCache);
00330        }
00331 }
00332 
00333 template <typename BP_FP_INT_TYPE>
00334 void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
00335 {
00336        btPoint3 clampedPoint(point);
00337        
00338 
00339 
00340        clampedPoint.setMax(m_worldAabbMin);
00341        clampedPoint.setMin(m_worldAabbMax);
00342 
00343        btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
00344        out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
00345        out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
00346        out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
00347        
00348 }
00349 
00350 
00351 template <typename BP_FP_INT_TYPE>
00352 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
00353 {
00354        assert(m_firstFreeHandle);
00355 
00356        BP_FP_INT_TYPE handle = m_firstFreeHandle;
00357        m_firstFreeHandle = getHandle(handle)->GetNextFree();
00358        m_numHandles++;
00359 
00360        return handle;
00361 }
00362 
00363 template <typename BP_FP_INT_TYPE>
00364 void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
00365 {
00366        assert(handle > 0 && handle < m_maxHandles);
00367 
00368        getHandle(handle)->SetNextFree(m_firstFreeHandle);
00369        m_firstFreeHandle = handle;
00370 
00371        m_numHandles--;
00372 }
00373 
00374 
00375 template <typename BP_FP_INT_TYPE>
00376 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
00377 {
00378        // quantize the bounds
00379        BP_FP_INT_TYPE min[3], max[3];
00380        quantize(min, aabbMin, 0);
00381        quantize(max, aabbMax, 1);
00382 
00383        // allocate a handle
00384        BP_FP_INT_TYPE handle = allocHandle();
00385        
00386 
00387        Handle* pHandle = getHandle(handle);
00388        
00389        pHandle->m_uniqueId = handle;
00390        //pHandle->m_pOverlaps = 0;
00391        pHandle->m_clientObject = pOwner;
00392        pHandle->m_collisionFilterGroup = collisionFilterGroup;
00393        pHandle->m_collisionFilterMask = collisionFilterMask;
00394        pHandle->m_multiSapParentProxy = multiSapProxy;
00395 
00396        // compute current limit of edge arrays
00397        BP_FP_INT_TYPE limit = m_numHandles * 2;
00398 
00399        
00400        // insert new edges just inside the max boundary edge
00401        for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
00402        {
00403 
00404               m_pHandles[0].m_maxEdges[axis] += 2;
00405 
00406               m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
00407 
00408               m_pEdges[axis][limit - 1].m_pos = min[axis];
00409               m_pEdges[axis][limit - 1].m_handle = handle;
00410 
00411               m_pEdges[axis][limit].m_pos = max[axis];
00412               m_pEdges[axis][limit].m_handle = handle;
00413 
00414               pHandle->m_minEdges[axis] = limit - 1;
00415               pHandle->m_maxEdges[axis] = limit;
00416        }
00417 
00418        // now sort the new edges to their correct position
00419        sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
00420        sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
00421        sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
00422        sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
00423        sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
00424        sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
00425 
00426 
00427        return handle;
00428 }
00429 
00430 
00431 template <typename BP_FP_INT_TYPE>
00432 void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
00433 {
00434 
00435        Handle* pHandle = getHandle(handle);
00436 
00437        //explicitly remove the pairs containing the proxy
00438        //we could do it also in the sortMinUp (passing true)
00439        //todo: compare performance
00440        if (!m_pairCache->hasDeferredRemoval())
00441        {
00442               m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
00443        }
00444 
00445        // compute current limit of edge arrays
00446        int limit = m_numHandles * 2;
00447        
00448        int axis;
00449 
00450        for (axis = 0;axis<3;axis++)
00451        {
00452               m_pHandles[0].m_maxEdges[axis] -= 2;
00453        }
00454 
00455        // remove the edges by sorting them up to the end of the list
00456        for ( axis = 0; axis < 3; axis++)
00457        {
00458               Edge* pEdges = m_pEdges[axis];
00459               BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
00460               pEdges[max].m_pos = m_handleSentinel;
00461 
00462               sortMaxUp(axis,max,dispatcher,false);
00463 
00464 
00465               BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
00466               pEdges[i].m_pos = m_handleSentinel;
00467 
00468 
00469               sortMinUp(axis,i,dispatcher,false);
00470 
00471               pEdges[limit-1].m_handle = 0;
00472               pEdges[limit-1].m_pos = m_handleSentinel;
00473               
00474 #ifdef DEBUG_BROADPHASE
00475                      debugPrintAxis(axis,false);
00476 #endif //DEBUG_BROADPHASE
00477 
00478 
00479        }
00480 
00481 
00482        // free the handle
00483        freeHandle(handle);
00484 
00485        
00486 }
00487 
00488 extern int gOverlappingPairs;
00489 //#include <stdio.h>
00490 
00491 template <typename BP_FP_INT_TYPE>
00492 void   btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
00493 {
00494 
00495        if (m_pairCache->hasDeferredRemoval())
00496        {
00497        
00498               btBroadphasePairArray&      overlappingPairArray = m_pairCache->getOverlappingPairArray();
00499 
00500               //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
00501               overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00502 
00503               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00504               m_invalidPair = 0;
00505 
00506               
00507               int i;
00508 
00509               btBroadphasePair previousPair;
00510               previousPair.m_pProxy0 = 0;
00511               previousPair.m_pProxy1 = 0;
00512               previousPair.m_algorithm = 0;
00513               
00514               
00515               for (i=0;i<overlappingPairArray.size();i++)
00516               {
00517               
00518                      btBroadphasePair& pair = overlappingPairArray[i];
00519 
00520                      bool isDuplicate = (pair == previousPair);
00521 
00522                      previousPair = pair;
00523 
00524                      bool needsRemoval = false;
00525 
00526                      if (!isDuplicate)
00527                      {
00528                             bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
00529 
00530                             if (hasOverlap)
00531                             {
00532                                    needsRemoval = false;//callback->processOverlap(pair);
00533                             } else
00534                             {
00535                                    needsRemoval = true;
00536                             }
00537                      } else
00538                      {
00539                             //remove duplicate
00540                             needsRemoval = true;
00541                             //should have no algorithm
00542                             btAssert(!pair.m_algorithm);
00543                      }
00544                      
00545                      if (needsRemoval)
00546                      {
00547                             m_pairCache->cleanOverlappingPair(pair,dispatcher);
00548 
00549               //            m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
00550               //            m_overlappingPairArray.pop_back();
00551                             pair.m_pProxy0 = 0;
00552                             pair.m_pProxy1 = 0;
00553                             m_invalidPair++;
00554                             gOverlappingPairs--;
00555                      } 
00556                      
00557               }
00558 
00560        #define CLEAN_INVALID_PAIRS 1
00561        #ifdef CLEAN_INVALID_PAIRS
00562 
00563               //perform a sort, to sort 'invalid' pairs to the end
00564               overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
00565 
00566               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
00567               m_invalidPair = 0;
00568        #endif//CLEAN_INVALID_PAIRS
00569               
00570               //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
00571        }
00572 
00573 
00574 
00575        
00576 
00577 }
00578 
00579 
00580 template <typename BP_FP_INT_TYPE>
00581 bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
00582 {
00583        const Handle* pHandleA = static_cast<Handle*>(proxy0);
00584        const Handle* pHandleB = static_cast<Handle*>(proxy1);
00585        
00586        //optimization 1: check the array index (memory address), instead of the m_pos
00587 
00588        for (int axis = 0; axis < 3; axis++)
00589        { 
00590               if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
00591                      pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
00592               { 
00593                      return false; 
00594               } 
00595        } 
00596        return true;
00597 }
00598 
00599 template <typename BP_FP_INT_TYPE>
00600 bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
00601 {
00602        //optimization 1: check the array index (memory address), instead of the m_pos
00603 
00604        for (int axis = 0; axis < 3; axis++)
00605        { 
00606               if (axis != ignoreAxis)
00607               {
00608                      if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
00609                             pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
00610                      { 
00611                             return false; 
00612                      } 
00613               }
00614        } 
00615 
00616        //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
00617 
00618        /*for (int axis = 0; axis < 3; axis++)
00619        {
00620               if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
00621                      m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
00622               {
00623                      return false;
00624               }
00625        }
00626        */
00627 
00628        return true;
00629 }
00630 
00631 template <typename BP_FP_INT_TYPE>
00632 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
00633 {
00634 //     assert(bounds.IsFinite());
00635        //assert(bounds.HasVolume());
00636 
00637        Handle* pHandle = getHandle(handle);
00638 
00639        // quantize the new bounds
00640        BP_FP_INT_TYPE min[3], max[3];
00641        quantize(min, aabbMin, 0);
00642        quantize(max, aabbMax, 1);
00643 
00644        // update changed edges
00645        for (int axis = 0; axis < 3; axis++)
00646        {
00647               BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
00648               BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
00649 
00650               int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
00651               int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
00652 
00653               m_pEdges[axis][emin].m_pos = min[axis];
00654               m_pEdges[axis][emax].m_pos = max[axis];
00655 
00656               // expand (only adds overlaps)
00657               if (dmin < 0)
00658                      sortMinDown(axis, emin,dispatcher,true);
00659 
00660               if (dmax > 0)
00661                      sortMaxUp(axis, emax,dispatcher,true);
00662 
00663               // shrink (only removes overlaps)
00664               if (dmin > 0)
00665                      sortMinUp(axis, emin,dispatcher,true);
00666 
00667               if (dmax < 0)
00668                      sortMaxDown(axis, emax,dispatcher,true);
00669 
00670 #ifdef DEBUG_BROADPHASE
00671        debugPrintAxis(axis);
00672 #endif //DEBUG_BROADPHASE
00673        }
00674 
00675        
00676 }
00677 
00678 
00679 
00680 
00681 // sorting a min edge downwards can only ever *add* overlaps
00682 template <typename BP_FP_INT_TYPE>
00683 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
00684 {
00685 
00686        Edge* pEdge = m_pEdges[axis] + edge;
00687        Edge* pPrev = pEdge - 1;
00688        Handle* pHandleEdge = getHandle(pEdge->m_handle);
00689 
00690        while (pEdge->m_pos < pPrev->m_pos)
00691        {
00692               Handle* pHandlePrev = getHandle(pPrev->m_handle);
00693 
00694               if (pPrev->IsMax())
00695               {
00696                      // if previous edge is a maximum check the bounds and add an overlap if necessary
00697                      if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
00698                      {
00699                             m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
00700                             if (m_userPairCallback)
00701                                    m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev);
00702 
00703                             //AddOverlap(pEdge->m_handle, pPrev->m_handle);
00704 
00705                      }
00706 
00707                      // update edge reference in other handle
00708                      pHandlePrev->m_maxEdges[axis]++;
00709               }
00710               else
00711                      pHandlePrev->m_minEdges[axis]++;
00712 
00713               pHandleEdge->m_minEdges[axis]--;
00714 
00715               // swap the edges
00716               Edge swap = *pEdge;
00717               *pEdge = *pPrev;
00718               *pPrev = swap;
00719 
00720               // decrement
00721               pEdge--;
00722               pPrev--;
00723        }
00724 
00725 #ifdef DEBUG_BROADPHASE
00726        debugPrintAxis(axis);
00727 #endif //DEBUG_BROADPHASE
00728 
00729 }
00730 
00731 // sorting a min edge upwards can only ever *remove* overlaps
00732 template <typename BP_FP_INT_TYPE>
00733 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
00734 {
00735        Edge* pEdge = m_pEdges[axis] + edge;
00736        Edge* pNext = pEdge + 1;
00737        Handle* pHandleEdge = getHandle(pEdge->m_handle);
00738 
00739        while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
00740        {
00741               Handle* pHandleNext = getHandle(pNext->m_handle);
00742 
00743               if (pNext->IsMax())
00744               {
00745 
00746                      // if next edge is maximum remove any overlap between the two handles
00747                      if (updateOverlaps)
00748                      {
00749                             Handle* handle0 = getHandle(pEdge->m_handle);
00750                             Handle* handle1 = getHandle(pNext->m_handle);
00751 
00752                             m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);       
00753                             if (m_userPairCallback)
00754                                    m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
00755                             
00756                      }
00757 
00758 
00759                      // update edge reference in other handle
00760                      pHandleNext->m_maxEdges[axis]--;
00761               }
00762               else
00763                      pHandleNext->m_minEdges[axis]--;
00764 
00765               pHandleEdge->m_minEdges[axis]++;
00766 
00767               // swap the edges
00768               Edge swap = *pEdge;
00769               *pEdge = *pNext;
00770               *pNext = swap;
00771 
00772               // increment
00773               pEdge++;
00774               pNext++;
00775        }
00776 
00777 
00778 }
00779 
00780 // sorting a max edge downwards can only ever *remove* overlaps
00781 template <typename BP_FP_INT_TYPE>
00782 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
00783 {
00784 
00785        Edge* pEdge = m_pEdges[axis] + edge;
00786        Edge* pPrev = pEdge - 1;
00787        Handle* pHandleEdge = getHandle(pEdge->m_handle);
00788 
00789        while (pEdge->m_pos < pPrev->m_pos)
00790        {
00791               Handle* pHandlePrev = getHandle(pPrev->m_handle);
00792 
00793               if (!pPrev->IsMax())
00794               {
00795                      // if previous edge was a minimum remove any overlap between the two handles
00796                      if (updateOverlaps)
00797                      {
00798                             //this is done during the overlappingpairarray iteration/narrowphase collision
00799 
00800                             Handle* handle0 = getHandle(pEdge->m_handle);
00801                             Handle* handle1 = getHandle(pPrev->m_handle);
00802                             m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
00803                             if (m_userPairCallback)
00804                                    m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
00805                      
00806 
00807 
00808                      }
00809 
00810                      // update edge reference in other handle
00811                      pHandlePrev->m_minEdges[axis]++;;
00812               }
00813               else
00814                      pHandlePrev->m_maxEdges[axis]++;
00815 
00816               pHandleEdge->m_maxEdges[axis]--;
00817 
00818               // swap the edges
00819               Edge swap = *pEdge;
00820               *pEdge = *pPrev;
00821               *pPrev = swap;
00822 
00823               // decrement
00824               pEdge--;
00825               pPrev--;
00826        }
00827 
00828        
00829 #ifdef DEBUG_BROADPHASE
00830        debugPrintAxis(axis);
00831 #endif //DEBUG_BROADPHASE
00832 
00833 }
00834 
00835 // sorting a max edge upwards can only ever *add* overlaps
00836 template <typename BP_FP_INT_TYPE>
00837 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
00838 {
00839        Edge* pEdge = m_pEdges[axis] + edge;
00840        Edge* pNext = pEdge + 1;
00841        Handle* pHandleEdge = getHandle(pEdge->m_handle);
00842 
00843        while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
00844        {
00845               Handle* pHandleNext = getHandle(pNext->m_handle);
00846 
00847               if (!pNext->IsMax())
00848               {
00849                      // if next edge is a minimum check the bounds and add an overlap if necessary
00850                      if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
00851                      {
00852                             Handle* handle0 = getHandle(pEdge->m_handle);
00853                             Handle* handle1 = getHandle(pNext->m_handle);
00854                             m_pairCache->addOverlappingPair(handle0,handle1);
00855                             if (m_userPairCallback)
00856                                    m_userPairCallback->addOverlappingPair(handle0,handle1);
00857                      }
00858 
00859                      // update edge reference in other handle
00860                      pHandleNext->m_minEdges[axis]--;
00861               }
00862               else
00863                      pHandleNext->m_maxEdges[axis]--;
00864 
00865               pHandleEdge->m_maxEdges[axis]++;
00866 
00867               // swap the edges
00868               Edge swap = *pEdge;
00869               *pEdge = *pNext;
00870               *pNext = swap;
00871 
00872               // increment
00873               pEdge++;
00874               pNext++;
00875        }
00876        
00877 }
00878 
00879 
00880 
00882 
00883 
00887 class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
00888 {
00889 public:
00890 
00891        btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
00892 
00893 };
00894 
00898 class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
00899 {
00900 public:
00901 
00902        bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
00903 
00904 };
00905 
00906 #endif
00907