Blender V2.61 - r43446

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