Changeset 2430 for code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h

Timestamp:

Dec 13, 2008, 11:45:51 PM (15 years ago)

Author:

rgrieder

Message:

Updated to Bullet 2.73 (first part).

File:

• code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h (modified) (22 diffs)

code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -20 +20 @@
 #define AXIS_SWEEP_3_H
 
-#include "LinearMath/btPoint3.h"
 #include "LinearMath/btVector3.h"
 #include "btOverlappingPairCache.h"
@@ -26 +25 @@
 #include "btBroadphaseProxy.h"
 #include "btOverlappingPairCallback.h"
+#include "btDbvtBroadphase.h"
 
 //#define DEBUG_BROADPHASE 1
@@ -62 +62 @@
                 BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];            // 6 * 2 = 12
 //              BP_FP_INT_TYPE m_uniqueId;
-                BP_FP_INT_TYPE m_pad;
-                
+                btBroadphaseProxy*      m_dbvtProxy;//for faster raycast
                 //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
         
@@ -72 +71 @@
         
 protected:
-        btPoint3 m_worldAabbMin;                                                // overall system bounds
-        btPoint3 m_worldAabbMax;                                                // overall system bounds
+        btVector3 m_worldAabbMin;                                               // overall system bounds
+        btVector3 m_worldAabbMax;                                               // overall system bounds
 
         btVector3 m_quantize;                                           // scaling factor for quantization
@@ -95 +94 @@
         int     m_invalidPair;
 
+        ///additional dynamic aabb structure, used to accelerate ray cast queries.
+        ///can be disabled using a optional argument in the constructor
+        btDbvtBroadphase*       m_raycastAccelerator;
+        btOverlappingPairCache* m_nullPairCache;
+
+
         // allocation/deallocation
         BP_FP_INT_TYPE allocHandle();
@@ -109 +114 @@
         //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
 
-        void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
+        
 
         void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
@@ -118 +123 @@
 public:
 
-        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);
+        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);
 
         virtual ~btAxisSweep3Internal();
@@ -129 +134 @@
         virtual void    calculateOverlappingPairs(btDispatcher* dispatcher);
         
-        BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+        BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
         void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
-        void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
+        void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
         SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+
+        void resetPool();
 
         void    processAllOverlappingPairs(btOverlapCallback* callback);
@@ -140 +147 @@
         virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
         virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+        virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+        
+        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));
+        
+        void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
+        ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
+        void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
         
         bool    testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
@@ -218 +232 @@
                 
                 Handle* handle = getHandle(handleId);
-                                
+                
+                if (m_raycastAccelerator)
+                {
+                        btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
+                        handle->m_dbvtProxy = rayProxy;
+                }
                 return handle;
 }
@@ -228 +247 @@
 {
         Handle* handle = static_cast<Handle*>(proxy);
+        if (m_raycastAccelerator)
+                m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
         removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
 }
@@ -235 +256 @@
 {
         Handle* handle = static_cast<Handle*>(proxy);
+        handle->m_aabbMin = aabbMin;
+        handle->m_aabbMax = aabbMax;
         updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
-
-}
-
-
-
-
-
-template <typename BP_FP_INT_TYPE>
-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 )
+        if (m_raycastAccelerator)
+                m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
+
+}
+
+template <typename BP_FP_INT_TYPE>
+void    btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+        if (m_raycastAccelerator)
+        {
+                m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
+        } else
+        {
+                //choose axis?
+                BP_FP_INT_TYPE axis = 0;
+                //for each proxy
+                for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
+                {
+                        if (m_pEdges[axis][i].IsMax())
+                        {
+                                rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
+                        }
+                }
+        }
+}
+
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+        Handle* pHandle = static_cast<Handle*>(proxy);
+        aabbMin = pHandle->m_aabbMin;
+        aabbMax = pHandle->m_aabbMax;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+        Handle* pHandle = static_cast<Handle*>(proxy);
+
+        unsigned short vecInMin[3];
+        unsigned short vecInMax[3];
+
+        vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
+        vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
+        vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
+        vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
+        vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
+        vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
+        
+        aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
+        aabbMin += m_worldAabbMin;
+        
+        aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
+        aabbMax += m_worldAabbMin;
+}
+
+
+
+
+template <typename BP_FP_INT_TYPE>
+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)
 :m_bpHandleMask(handleMask),
 m_handleSentinel(handleSentinel),
@@ -250 +328 @@
 m_userPairCallback(0),
 m_ownsPairCache(false),
-m_invalidPair(0)
+m_invalidPair(0),
+m_raycastAccelerator(0)
 {
         BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
@@ -259 +338 @@
                 m_pairCache = new(ptr) btHashedOverlappingPairCache();
                 m_ownsPairCache = true;
+        }
+
+        if (!disableRaycastAccelerator)
+        {
+                m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache();
+                m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache);
+                m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
         }
 
@@ -321 +407 @@
 btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
 {
-        
+        if (m_raycastAccelerator)
+        {
+                m_nullPairCache->~btOverlappingPairCache();
+                btAlignedFree(m_nullPairCache);
+                m_raycastAccelerator->~btDbvtBroadphase();
+                btAlignedFree (m_raycastAccelerator);
+        }
+
         for (int i = 2; i >= 0; i--)
         {
@@ -336 +429 @@
 
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
 {
 #ifdef OLD_CLAMPING_METHOD
         ///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]
         ///see http://code.google.com/p/bullet/issues/detail?id=87
-        btPoint3 clampedPoint(point);
+        btVector3 clampedPoint(point);
         clampedPoint.setMax(m_worldAabbMin);
         clampedPoint.setMin(m_worldAabbMax);
@@ -382 +475 @@
 
 template <typename BP_FP_INT_TYPE>
-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)
+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)
 {
         // quantize the bounds
@@ -445 +538 @@
         //explicitly remove the pairs containing the proxy
         //we could do it also in the sortMinUp (passing true)
-        //todo: compare performance
+        ///@todo: compare performance
         if (!m_pairCache->hasDeferredRemoval())
         {
@@ -493 +586 @@
         
 }
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool()
+{
+        if (m_numHandles == 0)
+        {
+                m_firstFreeHandle = 1;
+                {
+                        for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
+                                m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+                        m_pHandles[m_maxHandles - 1].SetNextFree(0);
+                }
+        }
+}       
+
 
 extern int gOverlappingPairs;
@@ -621 +729 @@
 
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
 {
 //      assert(bounds.IsFinite());
@@ -900 +1008 @@
 public:
 
-        btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
+        btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
 
 };
@@ -911 +1019 @@
 public:
 
-        bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
+        bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
 
 };