Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Dec 13, 2008, 11:45:51 PM (16 years ago)
Author:
rgrieder
Message:

Updated to Bullet 2.73 (first part).

Location:
code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision
Files:
17 edited

Legend:

Unmodified
Added
Removed

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

    r2192 r2430  
    2222#include <assert.h>
    2323
    24 btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache)
    25 :btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache)
     24btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
     25:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
    2626{
    2727        // 1 handle is reserved as sentinel
    … …  
    3131
    3232
    33 bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache )
    34 :btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache)
     33bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
     34:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator)
    3535{
    3636        // 1 handle is reserved as sentinel

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

    r2192 r2430  
    2020#define AXIS_SWEEP_3_H
    2121
    22 #include "LinearMath/btPoint3.h"
    2322#include "LinearMath/btVector3.h"
    2423#include "btOverlappingPairCache.h"
    … …  
    2625#include "btBroadphaseProxy.h"
    2726#include "btOverlappingPairCallback.h"
     27#include "btDbvtBroadphase.h"
    2828
    2929//#define DEBUG_BROADPHASE 1
    … …  
    6262                BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];            // 6 * 2 = 12
    6363//              BP_FP_INT_TYPE m_uniqueId;
    64                 BP_FP_INT_TYPE m_pad;
    65                
     64                btBroadphaseProxy*      m_dbvtProxy;//for faster raycast
    6665                //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
    6766       
    … …  
    7271       
    7372protected:
    74         btPoint3 m_worldAabbMin;                                                // overall system bounds
    75         btPoint3 m_worldAabbMax;                                                // overall system bounds
     73        btVector3 m_worldAabbMin;                                               // overall system bounds
     74        btVector3 m_worldAabbMax;                                               // overall system bounds
    7675
    7776        btVector3 m_quantize;                                           // scaling factor for quantization
    … …  
    9594        int     m_invalidPair;
    9695
     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
    97102        // allocation/deallocation
    98103        BP_FP_INT_TYPE allocHandle();
    … …  
    109114        //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
    110115
    111         void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
     116       
    112117
    113118        void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
    … …  
    118123public:
    119124
    120         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);
     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);
    121126
    122127        virtual ~btAxisSweep3Internal();
    … …  
    129134        virtual void    calculateOverlappingPairs(btDispatcher* dispatcher);
    130135       
    131         BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
     136        BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
    132137        void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
    133         void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
     138        void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
    134139        SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
     140
     141        void resetPool();
    135142
    136143        void    processAllOverlappingPairs(btOverlapCallback* callback);
    … …  
    140147        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    141148        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
     149        virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
     150       
     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;
    142156       
    143157        bool    testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
    … …  
    218232               
    219233                Handle* handle = getHandle(handleId);
    220                                
     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                }
    221240                return handle;
    222241}
    … …  
    228247{
    229248        Handle* handle = static_cast<Handle*>(proxy);
     249        if (m_raycastAccelerator)
     250                m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
    230251        removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
    231252}
    … …  
    235256{
    236257        Handle* handle = static_cast<Handle*>(proxy);
     258        handle->m_aabbMin = aabbMin;
     259        handle->m_aabbMax = aabbMax;
    237260        updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
    238 
    239 }
    240 
    241 
    242 
    243 
    244 
    245 template <typename BP_FP_INT_TYPE>
    246 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 )
     261        if (m_raycastAccelerator)
     262                m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
     263
     264}
     265
     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}
     286
     287
     288
     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}
     296
     297
     298template <typename BP_FP_INT_TYPE>
     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)
    247325:m_bpHandleMask(handleMask),
    248326m_handleSentinel(handleSentinel),
    … …  
    250328m_userPairCallback(0),
    251329m_ownsPairCache(false),
    252 m_invalidPair(0)
     330m_invalidPair(0),
     331m_raycastAccelerator(0)
    253332{
    254333        BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
    … …  
    259338                m_pairCache = new(ptr) btHashedOverlappingPairCache();
    260339                m_ownsPairCache = true;
     340        }
     341
     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
    261347        }
    262348
    … …  
    321407btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
    322408{
    323        
     409        if (m_raycastAccelerator)
     410        {
     411                m_nullPairCache->~btOverlappingPairCache();
     412                btAlignedFree(m_nullPairCache);
     413                m_raycastAccelerator->~btDbvtBroadphase();
     414                btAlignedFree (m_raycastAccelerator);
     415        }
     416
    324417        for (int i = 2; i >= 0; i--)
    325418        {
    … …  
    336429
    337430template <typename BP_FP_INT_TYPE>
    338 void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
     431void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
    339432{
    340433#ifdef OLD_CLAMPING_METHOD
    341434        ///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]
    342435        ///see http://code.google.com/p/bullet/issues/detail?id=87
    343         btPoint3 clampedPoint(point);
     436        btVector3 clampedPoint(point);
    344437        clampedPoint.setMax(m_worldAabbMin);
    345438        clampedPoint.setMin(m_worldAabbMax);
    … …  
    382475
    383476template <typename BP_FP_INT_TYPE>
    384 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)
     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)
    385478{
    386479        // quantize the bounds
    … …  
    445538        //explicitly remove the pairs containing the proxy
    446539        //we could do it also in the sortMinUp (passing true)
    447         //todo: compare performance
     540        ///@todo: compare performance
    448541        if (!m_pairCache->hasDeferredRemoval())
    449542        {
    … …  
    493586       
    494587}
     588
     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
    495603
    496604extern int gOverlappingPairs;
    … …  
    621729
    622730template <typename BP_FP_INT_TYPE>
    623 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
     731void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
    624732{
    625733//      assert(bounds.IsFinite());
    … …  
    9001008public:
    9011009
    902         btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
     1010        btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
    9031011
    9041012};
    … …  
    9111019public:
    9121020
    913         bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
     1021        bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
    9141022
    9151023};

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

    r2192 r2430  
    2222class btDispatcher;
    2323#include "btBroadphaseProxy.h"
     24
    2425class btOverlappingPairCache;
     26
     27
     28
     29struct  btBroadphaseRayCallback
     30{
     31        ///added some cached data to accelerate ray-AABB tests
     32        btVector3               m_rayDirectionInverse;
     33        unsigned int    m_signs[3];
     34        btScalar                m_lambda_max;
     35
     36        virtual ~btBroadphaseRayCallback() {}
     37        virtual bool    process(const btBroadphaseProxy* proxy) = 0;
     38};
    2539
    2640#include "LinearMath/btVector3.h"
    … …  
    3751        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
    3852        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
    39        
     53        virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
     54
     55        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)) = 0;
     56
    4057        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
    4158        virtual void    calculateOverlappingPairs(btDispatcher* dispatcher)=0;

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

    r2192 r2430  
    1818
    1919#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
     20#include "LinearMath/btVector3.h"
    2021#include "LinearMath/btAlignedAllocator.h"
    2122
    … …  
    2425/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
    2526/// to facilitate type checking
     27/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code
    2628enum BroadphaseNativeTypes
    2729{
    … …  
    3335        CONVEX_HULL_SHAPE_PROXYTYPE,
    3436        CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
     37        CUSTOM_POLYHEDRAL_SHAPE_TYPE,
    3538//implicit convex shapes
    3639IMPLICIT_CONVEX_SHAPES_START_HERE,
    … …  
    4447        MINKOWSKI_SUM_SHAPE_PROXYTYPE,
    4548        MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
     49        CUSTOM_CONVEX_SHAPE_TYPE,
    4650//concave shapes
    4751CONCAVE_SHAPES_START_HERE,
    … …  
    6064        EMPTY_SHAPE_PROXYTYPE,
    6165        STATIC_PLANE_PROXYTYPE,
     66        CUSTOM_CONCAVE_SHAPE_TYPE,
    6267CONCAVE_SHAPES_END_HERE,
    6368
    … …  
    8893                DebrisFilter = 8,
    8994                        SensorTrigger = 16,
     95                        CharacterFilter = 32,
    9096                AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
    9197        };
    … …  
    9399        //Usually the client btCollisionObject or Rigidbody class
    94100        void*   m_clientObject;
    95 
    96101        short int m_collisionFilterGroup;
    97102        short int m_collisionFilterMask;
    98 
    99103        void*   m_multiSapParentProxy;         
    100 
    101 
    102104        int                     m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
     105
     106        btVector3       m_aabbMin;
     107        btVector3       m_aabbMax;
    103108
    104109        SIMD_FORCE_INLINE int getUid() const
    … …  
    112117        }
    113118
    114         btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
     119        btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
    115120                :m_clientObject(userPtr),
    116121                m_collisionFilterGroup(collisionFilterGroup),
    117                 m_collisionFilterMask(collisionFilterMask)
     122                m_collisionFilterMask(collisionFilterMask),
     123                m_aabbMin(aabbMin),
     124                m_aabbMax(aabbMax)
    118125        {
    119126                m_multiSapParentProxy = multiSapParentProxy;
    … …  
    164171                m_pProxy1(0),
    165172                m_algorithm(0),
    166                 m_userInfo(0)
     173                m_internalInfo1(0)
    167174        {
    168175        }
    … …  
    174181                                m_pProxy1(other.m_pProxy1),
    175182                                m_algorithm(other.m_algorithm),
    176                                 m_userInfo(other.m_userInfo)
     183                                m_internalInfo1(other.m_internalInfo1)
    177184        {
    178185        }
    … …  
    193200
    194201                m_algorithm = 0;
    195                 m_userInfo = 0;
     202                m_internalInfo1 = 0;
    196203
    197204        }
    … …  
    201208       
    202209        mutable btCollisionAlgorithm* m_algorithm;
    203         mutable void* m_userInfo;
     210        union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version.
    204211
    205212};

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp

    r2192 r2430  
    2424struct btDbvtNodeEnumerator : btDbvt::ICollide
    2525{
    26 tConstNodeArray nodes;
    27 void Process(const btDbvtNode* n) { nodes.push_back(n); }
     26        tConstNodeArray nodes;
     27        void Process(const btDbvtNode* n) { nodes.push_back(n); }
    2828};
    2929
    … …  
    3131static DBVT_INLINE int                  indexof(const btDbvtNode* node)
    3232{
    33 return(node->parent->childs[1]==node);
     33        return(node->parent->childs[1]==node);
    3434}
    3535
    3636//
    3737static DBVT_INLINE btDbvtVolume merge(  const btDbvtVolume& a,
    38                                                                                 const btDbvtVolume& b)
    39 {
    40 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
    41 DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)];
    42 btDbvtVolume&   res=*(btDbvtVolume*)locals;
     38                                                                          const btDbvtVolume& b)
     39{
     40#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
     41        ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
     42        btDbvtVolume&   res=*(btDbvtVolume*)locals;
    4343#else
    44 btDbvtVolume    res;
     44                btDbvtVolume    res;
    4545#endif
    46 Merge(a,b,res);
    47 return(res);
     46        Merge(a,b,res);
     47        return(res);
    4848}
    4949
    … …  
    5151static DBVT_INLINE btScalar             size(const btDbvtVolume& a)
    5252{
    53 const btVector3 edges=a.Lengths();
    54 return( edges.x()*edges.y()*edges.z()+
     53        const btVector3 edges=a.Lengths();
     54        return( edges.x()*edges.y()*edges.z()+
    5555                edges.x()+edges.y()+edges.z());
    5656}
    … …  
    5959static void                                             getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
    6060{
    61 if(node->isinternal())
    62         {
    63         getmaxdepth(node->childs[0],depth+1,maxdepth);
    64         getmaxdepth(node->childs[0],depth+1,maxdepth);
     61        if(node->isinternal())
     62        {
     63                getmaxdepth(node->childs[0],depth+1,maxdepth);
     64                getmaxdepth(node->childs[0],depth+1,maxdepth);
    6565        } else maxdepth=btMax(maxdepth,depth);
    6666}
    … …  
    6868//
    6969static DBVT_INLINE void                 deletenode(     btDbvt* pdbvt,
    70                                                                                         btDbvtNode* node)
    71 {
    72 btAlignedFree(pdbvt->m_free);
    73 pdbvt->m_free=node;
    74 }
    75        
     70                                                                                   btDbvtNode* node)
     71{
     72        btAlignedFree(pdbvt->m_free);
     73        pdbvt->m_free=node;
     74}
     75
    7676//
    7777static void                                             recursedeletenode(      btDbvt* pdbvt,
    78                                                                                                         btDbvtNode* node)
    79 {
    80 if(!node->isleaf())
    81         {
    82         recursedeletenode(pdbvt,node->childs[0]);
    83         recursedeletenode(pdbvt,node->childs[1]);
    84         }
    85 if(node==pdbvt->m_root) pdbvt->m_root=0;
    86 deletenode(pdbvt,node);
     78                                                                                                  btDbvtNode* node)
     79{
     80        if(!node->isleaf())
     81        {
     82                recursedeletenode(pdbvt,node->childs[0]);
     83                recursedeletenode(pdbvt,node->childs[1]);
     84        }
     85        if(node==pdbvt->m_root) pdbvt->m_root=0;
     86        deletenode(pdbvt,node);
    8787}
    8888
    8989//
    9090static DBVT_INLINE btDbvtNode*  createnode(     btDbvt* pdbvt,
    91                                                                                         btDbvtNode* parent,
    92                                                                                         void* data)
    93 {
    94 btDbvtNode*     node;
    95 if(pdbvt->m_free)
     91                                                                                   btDbvtNode* parent,
     92                                                                                   void* data)
     93{
     94        btDbvtNode*     node;
     95        if(pdbvt->m_free)
    9696        { node=pdbvt->m_free;pdbvt->m_free=0; }
    9797        else
    9898        { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
    99 node->parent    =       parent;
    100 node->data              =       data;
    101 node->childs[1] =       0;
    102 return(node);
     99        node->parent    =       parent;
     100        node->data              =       data;
     101        node->childs[1] =       0;
     102        return(node);
    103103}
    104104
    105105//
    106106static DBVT_INLINE btDbvtNode*  createnode(     btDbvt* pdbvt,
    107                                                                                         btDbvtNode* parent,
    108                                                                                         const btDbvtVolume& volume,
    109                                                                                         void* data)
    110 {
    111 btDbvtNode*     node=createnode(pdbvt,parent,data);
    112 node->volume=volume;
    113 return(node);
     107                                                                                   btDbvtNode* parent,
     108                                                                                   const btDbvtVolume& volume,
     109                                                                                   void* data)
     110{
     111        btDbvtNode*     node=createnode(pdbvt,parent,data);
     112        node->volume=volume;
     113        return(node);
    114114}
    115115
    116116//
    117117static DBVT_INLINE btDbvtNode*  createnode(     btDbvt* pdbvt,
    118                                                                                         btDbvtNode* parent,
    119                                                                                         const btDbvtVolume& volume0,
    120                                                                                         const btDbvtVolume& volume1,
    121                                                                                         void* data)
    122 {
    123 btDbvtNode*     node=createnode(pdbvt,parent,data);
    124 Merge(volume0,volume1,node->volume);
    125 return(node);
     118                                                                                   btDbvtNode* parent,
     119                                                                                   const btDbvtVolume& volume0,
     120                                                                                   const btDbvtVolume& volume1,
     121                                                                                   void* data)
     122{
     123        btDbvtNode*     node=createnode(pdbvt,parent,data);
     124        Merge(volume0,volume1,node->volume);
     125        return(node);
    126126}
    127127
    128128//
    129129static void                                             insertleaf(     btDbvt* pdbvt,
    130                                                                                         btDbvtNode* root,
    131                                                                                         btDbvtNode* leaf)
    132 {
    133 if(!pdbvt->m_root)
    134         {
    135         pdbvt->m_root   =       leaf;
    136         leaf->parent    =       0;
     130                                                                                   btDbvtNode* root,
     131                                                                                   btDbvtNode* leaf)
     132{
     133        if(!pdbvt->m_root)
     134        {
     135                pdbvt->m_root   =       leaf;
     136                leaf->parent    =       0;
    137137        }
    138138        else
    139139        {
    140         if(!root->isleaf())
    141                 {
    142                 do      {
    143                         root=root->childs[Select(       leaf->volume,
    144                                                                                 root->childs[0]->volume,
    145                                                                                 root->childs[1]->volume)];
     140                if(!root->isleaf())
     141                {
     142                        do      {
     143                                root=root->childs[Select(       leaf->volume,
     144                                        root->childs[0]->volume,
     145                                        root->childs[1]->volume)];
    146146                        } while(!root->isleaf());
    147147                }
    148         btDbvtNode*     prev=root->parent;
    149         btDbvtNode*     node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
    150         if(prev)
    151                 {
    152                 prev->childs[indexof(root)]     =       node;
    153                 node->childs[0]                         =       root;root->parent=node;
    154                 node->childs[1]                         =       leaf;leaf->parent=node;
    155                 do      {
    156                         if(!prev->volume.Contain(node->volume))
     148                btDbvtNode*     prev=root->parent;
     149                btDbvtNode*     node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
     150                if(prev)
     151                {
     152                        prev->childs[indexof(root)]     =       node;
     153                        node->childs[0]                         =       root;root->parent=node;
     154                        node->childs[1]                         =       leaf;leaf->parent=node;
     155                        do      {
     156                                if(!prev->volume.Contain(node->volume))
     157                                        Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
     158                                else
     159                                        break;
     160                                node=prev;
     161                        } while(0!=(prev=node->parent));
     162                }
     163                else
     164                {
     165                        node->childs[0] =       root;root->parent=node;
     166                        node->childs[1] =       leaf;leaf->parent=node;
     167                        pdbvt->m_root   =       node;
     168                }
     169        }
     170}
     171
     172//
     173static btDbvtNode*                              removeleaf(     btDbvt* pdbvt,
     174                                                                                   btDbvtNode* leaf)
     175{
     176        if(leaf==pdbvt->m_root)
     177        {
     178                pdbvt->m_root=0;
     179                return(0);
     180        }
     181        else
     182        {
     183                btDbvtNode*     parent=leaf->parent;
     184                btDbvtNode*     prev=parent->parent;
     185                btDbvtNode*     sibling=parent->childs[1-indexof(leaf)];                       
     186                if(prev)
     187                {
     188                        prev->childs[indexof(parent)]=sibling;
     189                        sibling->parent=prev;
     190                        deletenode(pdbvt,parent);
     191                        while(prev)
     192                        {
     193                                const btDbvtVolume      pb=prev->volume;
    157194                                Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
    158                                 else
    159                                 break;
    160                         node=prev;
    161                         } while(0!=(prev=node->parent));
    162                 }
    163                 else
    164                 {
    165                 node->childs[0] =       root;root->parent=node;
    166                 node->childs[1] =       leaf;leaf->parent=node;
    167                 pdbvt->m_root   =       node;
    168                 }
    169         }
    170 }
    171        
    172 //
    173 static btDbvtNode*                              removeleaf(     btDbvt* pdbvt,
    174                                                                                         btDbvtNode* leaf)
    175 {
    176 if(leaf==pdbvt->m_root)
    177         {
    178         pdbvt->m_root=0;
    179         return(0);
    180         }
    181         else
    182         {
    183         btDbvtNode*     parent=leaf->parent;
    184         btDbvtNode*     prev=parent->parent;
    185         btDbvtNode*     sibling=parent->childs[1-indexof(leaf)];                       
    186         if(prev)
    187                 {
    188                 prev->childs[indexof(parent)]=sibling;
    189                 sibling->parent=prev;
    190                 deletenode(pdbvt,parent);
    191                 while(prev)
    192                         {
    193                         const btDbvtVolume      pb=prev->volume;
    194                         Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
    195                         if(NotEqual(pb,prev->volume))
     195                                if(NotEqual(pb,prev->volume))
    196196                                {
    197                                 prev=prev->parent;
     197                                        prev=prev->parent;
    198198                                } else break;
    199199                        }
    200                 return(prev?prev:pdbvt->m_root);
     200                        return(prev?prev:pdbvt->m_root);
    201201                }
    202202                else
    203203                {                                                               
    204                 pdbvt->m_root=sibling;
    205                 sibling->parent=0;
    206                 deletenode(pdbvt,parent);
    207                 return(pdbvt->m_root);
     204                        pdbvt->m_root=sibling;
     205                        sibling->parent=0;
     206                        deletenode(pdbvt,parent);
     207                        return(pdbvt->m_root);
    208208                }                       
    209209        }
    … …  
    216216                                                                                        int depth=-1)
    217217{
    218 if(root->isinternal()&&depth)
    219         {
    220         fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
    221         fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
    222         deletenode(pdbvt,root);
     218        if(root->isinternal()&&depth)
     219        {
     220                fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
     221                fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
     222                deletenode(pdbvt,root);
    223223        }
    224224        else
    225225        {
    226         leaves.push_back(root);
     226                leaves.push_back(root);
    227227        }
    228228}
    … …  
    230230//
    231231static void                                             split(  const tNodeArray& leaves,
    232                                                                                 tNodeArray& left,
    233                                                                                 tNodeArray& right,
    234                                                                                 const btVector3& org,
    235                                                                                 const btVector3& axis)
    236 {
    237 left.resize(0);
    238 right.resize(0);
    239 for(int i=0,ni=leaves.size();i<ni;++i)
    240         {
    241         if(dot(axis,leaves[i]->volume.Center()-org)<0)
    242                 left.push_back(leaves[i]);
     232                                                                          tNodeArray& left,
     233                                                                          tNodeArray& right,
     234                                                                          const btVector3& org,
     235                                                                          const btVector3& axis)
     236{
     237        left.resize(0);
     238        right.resize(0);
     239        for(int i=0,ni=leaves.size();i<ni;++i)
     240        {
     241                if(dot(axis,leaves[i]->volume.Center()-org)<0)
     242                        left.push_back(leaves[i]);
    243243                else
    244                 right.push_back(leaves[i]);
     244                        right.push_back(leaves[i]);
    245245        }
    246246}
    … …  
    250250{
    251251#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
    252 DBVT_ALIGN char locals[sizeof(btDbvtVolume)];
    253 btDbvtVolume&   volume=*(btDbvtVolume*)locals;
    254 volume=leaves[0]->volume;
     252        ATTRIBUTE_ALIGNED16(char        locals[sizeof(btDbvtVolume)]);
     253        btDbvtVolume&   volume=*(btDbvtVolume*)locals;
     254        volume=leaves[0]->volume;
    255255#else
    256 btDbvtVolume volume=leaves[0]->volume;
     256        btDbvtVolume volume=leaves[0]->volume;
    257257#endif
    258 for(int i=1,ni=leaves.size();i<ni;++i)
    259         {
    260         Merge(volume,leaves[i]->volume,volume);
    261         }
    262 return(volume);
     258        for(int i=1,ni=leaves.size();i<ni;++i)
     259        {
     260                Merge(volume,leaves[i]->volume,volume);
     261        }
     262        return(volume);
    263263}
    264264
    265265//
    266266static void                                             bottomup(       btDbvt* pdbvt,
    267                                                                                         tNodeArray& leaves)
    268 {
    269 while(leaves.size()>1)
    270         {
    271         btScalar        minsize=SIMD_INFINITY;
    272         int                     minidx[2]={-1,-1};
    273         for(int i=0;i<leaves.size();++i)
    274                 {
    275                 for(int j=i+1;j<leaves.size();++j)
    276                         {
    277                         const btScalar  sz=size(merge(leaves[i]->volume,leaves[j]->volume));
    278                         if(sz<minsize)
     267                                                                                 tNodeArray& leaves)
     268{
     269        while(leaves.size()>1)
     270        {
     271                btScalar        minsize=SIMD_INFINITY;
     272                int                     minidx[2]={-1,-1};
     273                for(int i=0;i<leaves.size();++i)
     274                {
     275                        for(int j=i+1;j<leaves.size();++j)
     276                        {
     277                                const btScalar  sz=size(merge(leaves[i]->volume,leaves[j]->volume));
     278                                if(sz<minsize)
    279279                                {
    280                                 minsize         =       sz;
    281                                 minidx[0]       =       i;
    282                                 minidx[1]       =       j;
     280                                        minsize         =       sz;
     281                                        minidx[0]       =       i;
     282                                        minidx[1]       =       j;
    283283                                }
    284284                        }
    285285                }
    286         btDbvtNode*     n[]     =       {leaves[minidx[0]],leaves[minidx[1]]};
    287         btDbvtNode*     p       =       createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
    288         p->childs[0]            =       n[0];
    289         p->childs[1]            =       n[1];
    290         n[0]->parent            =       p;
    291         n[1]->parent            =       p;
    292         leaves[minidx[0]]       =       p;
    293         leaves.swap(minidx[1],leaves.size()-1);
    294         leaves.pop_back();
     286                btDbvtNode*     n[]     =       {leaves[minidx[0]],leaves[minidx[1]]};
     287                btDbvtNode*     p       =       createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
     288                p->childs[0]            =       n[0];
     289                p->childs[1]            =       n[1];
     290                n[0]->parent            =       p;
     291                n[1]->parent            =       p;
     292                leaves[minidx[0]]       =       p;
     293                leaves.swap(minidx[1],leaves.size()-1);
     294                leaves.pop_back();
    295295        }
    296296}
    … …  
    301301                                                                        int bu_treshold)
    302302{
    303 static const btVector3  axis[]={btVector3(1,0,0),
    304                                                                 btVector3(0,1,0),
    305                                                                 btVector3(0,0,1)};
    306 if(leaves.size()>1)
    307         {
    308         if(leaves.size()>bu_treshold)
    309                 {
    310                 const btDbvtVolume      vol=bounds(leaves);
    311                 const btVector3                 org=vol.Center();
    312                 tNodeArray                              sets[2];
    313                 int                                             bestaxis=-1;
    314                 int                                             bestmidp=leaves.size();
    315                 int                                             splitcount[3][2]={{0,0},{0,0},{0,0}};
    316                 int i;
    317                 for( i=0;i<leaves.size();++i)
    318                         {
    319                         const btVector3 x=leaves[i]->volume.Center()-org;
    320                         for(int j=0;j<3;++j)
     303        static const btVector3  axis[]={btVector3(1,0,0),
     304                btVector3(0,1,0),
     305                btVector3(0,0,1)};
     306        if(leaves.size()>1)
     307        {
     308                if(leaves.size()>bu_treshold)
     309                {
     310                        const btDbvtVolume      vol=bounds(leaves);
     311                        const btVector3                 org=vol.Center();
     312                        tNodeArray                              sets[2];
     313                        int                                             bestaxis=-1;
     314                        int                                             bestmidp=leaves.size();
     315                        int                                             splitcount[3][2]={{0,0},{0,0},{0,0}};
     316                        int i;
     317                        for( i=0;i<leaves.size();++i)
     318                        {
     319                                const btVector3 x=leaves[i]->volume.Center()-org;
     320                                for(int j=0;j<3;++j)
    321321                                {
    322                                 ++splitcount[j][dot(x,axis[j])>0?1:0];
     322                                        ++splitcount[j][dot(x,axis[j])>0?1:0];
    323323                                }
    324324                        }
    325                 for( i=0;i<3;++i)
    326                         {
    327                         if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
     325                        for( i=0;i<3;++i)
     326                        {
     327                                if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
    328328                                {
    329                                 const int       midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
    330                                 if(midp<bestmidp)
     329                                        const int       midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
     330                                        if(midp<bestmidp)
    331331                                        {
    332                                         bestaxis=i;
    333                                         bestmidp=midp;
     332                                                bestaxis=i;
     333                                                bestmidp=midp;
    334334                                        }
    335335                                }
    336336                        }
    337                 if(bestaxis>=0)
    338                         {
    339                         sets[0].reserve(splitcount[bestaxis][0]);
    340                         sets[1].reserve(splitcount[bestaxis][1]);
    341                         split(leaves,sets[0],sets[1],org,axis[bestaxis]);
     337                        if(bestaxis>=0)
     338                        {
     339                                sets[0].reserve(splitcount[bestaxis][0]);
     340                                sets[1].reserve(splitcount[bestaxis][1]);
     341                                split(leaves,sets[0],sets[1],org,axis[bestaxis]);
    342342                        }
    343343                        else
    344344                        {
    345                         sets[0].reserve(leaves.size()/2+1);
    346                         sets[1].reserve(leaves.size()/2);
    347                         for(int i=0,ni=leaves.size();i<ni;++i)
     345                                sets[0].reserve(leaves.size()/2+1);
     346                                sets[1].reserve(leaves.size()/2);
     347                                for(int i=0,ni=leaves.size();i<ni;++i)
    348348                                {
    349                                 sets[i&1].push_back(leaves[i]);
     349                                        sets[i&1].push_back(leaves[i]);
    350350                                }
    351351                        }
    352                 btDbvtNode*     node=createnode(pdbvt,0,vol,0);
    353                 node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
    354                 node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
    355                 node->childs[0]->parent=node;
    356                 node->childs[1]->parent=node;
    357                 return(node);
     352                        btDbvtNode*     node=createnode(pdbvt,0,vol,0);
     353                        node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
     354                        node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
     355                        node->childs[0]->parent=node;
     356                        node->childs[1]->parent=node;
     357                        return(node);
    358358                }
    359359                else
    360360                {
    361                 bottomup(pdbvt,leaves);
    362                 return(leaves[0]);
    363                 }
    364         }
    365 return(leaves[0]);
     361                        bottomup(pdbvt,leaves);
     362                        return(leaves[0]);
     363                }
     364        }
     365        return(leaves[0]);
    366366}
    367367
    … …  
    369369static DBVT_INLINE btDbvtNode*  sort(btDbvtNode* n,btDbvtNode*& r)
    370370{
    371 btDbvtNode*     p=n->parent;
    372 btAssert(n->isinternal());
    373 if(p>n)
    374         {
    375         const int               i=indexof(n);
    376         const int               j=1-i;
    377         btDbvtNode*     s=p->childs[j];
    378         btDbvtNode*     q=p->parent;
    379         btAssert(n==p->childs[i]);
    380         if(q) q->childs[indexof(p)]=n; else r=n;
    381         s->parent=n;
    382         p->parent=n;
    383         n->parent=q;
    384         p->childs[0]=n->childs[0];
    385         p->childs[1]=n->childs[1];
    386         n->childs[0]->parent=p;
    387         n->childs[1]->parent=p;
    388         n->childs[i]=p;
    389         n->childs[j]=s;
    390         btSwap(p->volume,n->volume);
    391         return(p);
    392         }
    393 return(n);
     371        btDbvtNode*     p=n->parent;
     372        btAssert(n->isinternal());
     373        if(p>n)
     374        {
     375                const int               i=indexof(n);
     376                const int               j=1-i;
     377                btDbvtNode*     s=p->childs[j];
     378                btDbvtNode*     q=p->parent;
     379                btAssert(n==p->childs[i]);
     380                if(q) q->childs[indexof(p)]=n; else r=n;
     381                s->parent=n;
     382                p->parent=n;
     383                n->parent=q;
     384                p->childs[0]=n->childs[0];
     385                p->childs[1]=n->childs[1];
     386                n->childs[0]->parent=p;
     387                n->childs[1]->parent=p;
     388                n->childs[i]=p;
     389                n->childs[j]=s;
     390                btSwap(p->volume,n->volume);
     391                return(p);
     392        }
     393        return(n);
    394394}
    395395
    … …  
    397397static DBVT_INLINE btDbvtNode*  walkup(btDbvtNode* n,int count)
    398398{
    399 while(n&&(count--)) n=n->parent;
    400 return(n);
     399        while(n&&(count--)) n=n->parent;
     400        return(n);
    401401}
    402402
    … …  
    406406
    407407//
    408                                 btDbvt::btDbvt()
    409 {
    410 m_root          =       0;
    411 m_free          =       0;
    412 m_lkhd          =       -1;
    413 m_leaves        =       0;
    414 m_opath         =       0;
    415 }
    416 
    417 //
    418                                 btDbvt::~btDbvt()
    419 {
    420 clear();
     408btDbvt::btDbvt()
     409{
     410        m_root          =       0;
     411        m_free          =       0;
     412        m_lkhd          =       -1;
     413        m_leaves        =       0;
     414        m_opath         =       0;
     415}
     416
     417//
     418btDbvt::~btDbvt()
     419{
     420        clear();
    421421}
    422422
    … …  
    424424void                    btDbvt::clear()
    425425{
    426 if(m_root)      recursedeletenode(this,m_root);
    427 btAlignedFree(m_free);
    428 m_free=0;
     426        if(m_root)      recursedeletenode(this,m_root);
     427        btAlignedFree(m_free);
     428        m_free=0;
    429429}
    430430
    … …  
    432432void                    btDbvt::optimizeBottomUp()
    433433{
    434 if(m_root)
    435         {
    436         tNodeArray leaves;
    437         leaves.reserve(m_leaves);
    438         fetchleaves(this,m_root,leaves);
    439         bottomup(this,leaves);
    440         m_root=leaves[0];
     434        if(m_root)
     435        {
     436                tNodeArray leaves;
     437                leaves.reserve(m_leaves);
     438                fetchleaves(this,m_root,leaves);
     439                bottomup(this,leaves);
     440                m_root=leaves[0];
    441441        }
    442442}
    … …  
    445445void                    btDbvt::optimizeTopDown(int bu_treshold)
    446446{
    447 if(m_root)
    448         {
    449         tNodeArray      leaves;
    450         leaves.reserve(m_leaves);
    451         fetchleaves(this,m_root,leaves);
    452         m_root=topdown(this,leaves,bu_treshold);
     447        if(m_root)
     448        {
     449                tNodeArray      leaves;
     450                leaves.reserve(m_leaves);
     451                fetchleaves(this,m_root,leaves);
     452                m_root=topdown(this,leaves,bu_treshold);
    453453        }
    454454}
    … …  
    457457void                    btDbvt::optimizeIncremental(int passes)
    458458{
    459 if(passes<0) passes=m_leaves;
    460 if(m_root&&(passes>0))
    461         {
    462         do      {
    463                 btDbvtNode*             node=m_root;
    464                 unsigned        bit=0;
    465                 while(node->isinternal())
    466                         {
    467                         node=sort(node,m_root)->childs[(m_opath>>bit)&1];
    468                         bit=(bit+1)&(sizeof(unsigned)*8-1);
    469                         }
    470                 update(node);
    471                 ++m_opath;
     459        if(passes<0) passes=m_leaves;
     460        if(m_root&&(passes>0))
     461        {
     462                do      {
     463                        btDbvtNode*             node=m_root;
     464                        unsigned        bit=0;
     465                        while(node->isinternal())
     466                        {
     467                                node=sort(node,m_root)->childs[(m_opath>>bit)&1];
     468                                bit=(bit+1)&(sizeof(unsigned)*8-1);
     469                        }
     470                        update(node);
     471                        ++m_opath;
    472472                } while(--passes);
    473473        }
    … …  
    477477btDbvtNode*     btDbvt::insert(const btDbvtVolume& volume,void* data)
    478478{
    479 btDbvtNode*     leaf=createnode(this,0,volume,data);
    480 insertleaf(this,m_root,leaf);
    481 ++m_leaves;
    482 return(leaf);
     479        btDbvtNode*     leaf=createnode(this,0,volume,data);
     480        insertleaf(this,m_root,leaf);
     481        ++m_leaves;
     482        return(leaf);
    483483}
    484484
    … …  
    486486void                    btDbvt::update(btDbvtNode* leaf,int lookahead)
    487487{
    488 btDbvtNode*     root=removeleaf(this,leaf);
    489 if(root)
    490         {
    491         if(lookahead>=0)
    492                 {
    493                 for(int i=0;(i<lookahead)&&root->parent;++i)
    494                         {
    495                         root=root->parent;
     488        btDbvtNode*     root=removeleaf(this,leaf);
     489        if(root)
     490        {
     491                if(lookahead>=0)
     492                {
     493                        for(int i=0;(i<lookahead)&&root->parent;++i)
     494                        {
     495                                root=root->parent;
    496496                        }
    497497                } else root=m_root;
    498498        }
    499 insertleaf(this,root,leaf);
    500 }
    501 
    502 //
    503 void                    btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume)
    504 {
    505 btDbvtNode*     root=removeleaf(this,leaf);
    506 if(root)
    507         {
    508         if(m_lkhd>=0)
    509                 {
    510                 for(int i=0;(i<m_lkhd)&&root->parent;++i)
    511                         {
    512                         root=root->parent;
     499        insertleaf(this,root,leaf);
     500}
     501
     502//
     503void                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume)
     504{
     505        btDbvtNode*     root=removeleaf(this,leaf);
     506        if(root)
     507        {
     508                if(m_lkhd>=0)
     509                {
     510                        for(int i=0;(i<m_lkhd)&&root->parent;++i)
     511                        {
     512                                root=root->parent;
    513513                        }
    514514                } else root=m_root;
    515515        }
    516 leaf->volume=volume;
    517 insertleaf(this,root,leaf);
    518 }
    519 
    520 //
    521 bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin)
    522 {
    523 if(leaf->volume.Contain(volume)) return(false);
    524 volume.Expand(btVector3(margin,margin,margin));
    525 volume.SignedExpand(velocity);
    526 update(leaf,volume);
    527 return(true);
    528 }
    529 
    530 //
    531 bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity)
    532 {
    533 if(leaf->volume.Contain(volume)) return(false);
    534 volume.SignedExpand(velocity);
    535 update(leaf,volume);
    536 return(true);
    537 }
    538 
    539 //
    540 bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin)
    541 {
    542 if(leaf->volume.Contain(volume)) return(false);
    543 volume.Expand(btVector3(margin,margin,margin));
    544 update(leaf,volume);
    545 return(true);
     516        leaf->volume=volume;
     517        insertleaf(this,root,leaf);
     518}
     519
     520//
     521bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin)
     522{
     523        if(leaf->volume.Contain(volume)) return(false);
     524        volume.Expand(btVector3(margin,margin,margin));
     525        volume.SignedExpand(velocity);
     526        update(leaf,volume);
     527        return(true);
     528}
     529
     530//
     531bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity)
     532{
     533        if(leaf->volume.Contain(volume)) return(false);
     534        volume.SignedExpand(velocity);
     535        update(leaf,volume);
     536        return(true);
     537}
     538
     539//
     540bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin)
     541{
     542        if(leaf->volume.Contain(volume)) return(false);
     543        volume.Expand(btVector3(margin,margin,margin));
     544        update(leaf,volume);
     545        return(true);
    546546}
    547547
    … …  
    549549void                    btDbvt::remove(btDbvtNode* leaf)
    550550{
    551 removeleaf(this,leaf);
    552 deletenode(this,leaf);
    553 --m_leaves;
     551        removeleaf(this,leaf);
     552        deletenode(this,leaf);
     553        --m_leaves;
    554554}
    555555
    … …  
    557557void                    btDbvt::write(IWriter* iwriter) const
    558558{
    559 btDbvtNodeEnumerator    nodes;
    560 nodes.nodes.reserve(m_leaves*2);
    561 enumNodes(m_root,nodes);
    562 iwriter->Prepare(m_root,nodes.nodes.size());
    563 for(int i=0;i<nodes.nodes.size();++i)
    564         {
    565         const btDbvtNode* n=nodes.nodes[i];
    566         int                     p=-1;
    567         if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
    568         if(n->isinternal())
    569                 {
    570                 const int       c0=nodes.nodes.findLinearSearch(n->childs[0]);
    571                 const int       c1=nodes.nodes.findLinearSearch(n->childs[1]);
    572                 iwriter->WriteNode(n,i,p,c0,c1);
     559        btDbvtNodeEnumerator    nodes;
     560        nodes.nodes.reserve(m_leaves*2);
     561        enumNodes(m_root,nodes);
     562        iwriter->Prepare(m_root,nodes.nodes.size());
     563        for(int i=0;i<nodes.nodes.size();++i)
     564        {
     565                const btDbvtNode* n=nodes.nodes[i];
     566                int                     p=-1;
     567                if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
     568                if(n->isinternal())
     569                {
     570                        const int       c0=nodes.nodes.findLinearSearch(n->childs[0]);
     571                        const int       c1=nodes.nodes.findLinearSearch(n->childs[1]);
     572                        iwriter->WriteNode(n,i,p,c0,c1);
    573573                }
    574574                else
    575575                {
    576                 iwriter->WriteLeaf(n,i,p);
     576                        iwriter->WriteLeaf(n,i,p);
    577577                }       
    578578        }
    … …  
    582582void                    btDbvt::clone(btDbvt& dest,IClone* iclone) const
    583583{
    584 dest.clear();
    585 if(m_root!=0)
     584        dest.clear();
     585        if(m_root!=0)
    586586        {       
    587         btAlignedObjectArray<sStkCLN>   stack;
    588         stack.reserve(m_leaves);
    589         stack.push_back(sStkCLN(m_root,0));
    590         do      {
    591                 const int               i=stack.size()-1;
    592                 const sStkCLN   e=stack[i];
    593                 btDbvtNode*                     n=createnode(&dest,e.parent,e.node->volume,e.node->data);
    594                 stack.pop_back();
    595                 if(e.parent!=0)
    596                         e.parent->childs[i&1]=n;
     587                btAlignedObjectArray<sStkCLN>   stack;
     588                stack.reserve(m_leaves);
     589                stack.push_back(sStkCLN(m_root,0));
     590                do      {
     591                        const int               i=stack.size()-1;
     592                        const sStkCLN   e=stack[i];
     593                        btDbvtNode*                     n=createnode(&dest,e.parent,e.node->volume,e.node->data);
     594                        stack.pop_back();
     595                        if(e.parent!=0)
     596                                e.parent->childs[i&1]=n;
    597597                        else
    598                         dest.m_root=n;
    599                 if(e.node->isinternal())
    600                         {
    601                         stack.push_back(sStkCLN(e.node->childs[0],n));
    602                         stack.push_back(sStkCLN(e.node->childs[1],n));
     598                                dest.m_root=n;
     599                        if(e.node->isinternal())
     600                        {
     601                                stack.push_back(sStkCLN(e.node->childs[0],n));
     602                                stack.push_back(sStkCLN(e.node->childs[1],n));
    603603                        }
    604604                        else
    605605                        {
    606                         iclone->CloneLeaf(n);
     606                                iclone->CloneLeaf(n);
    607607                        }
    608608                } while(stack.size()>0);
    … …  
    613613int                             btDbvt::maxdepth(const btDbvtNode* node)
    614614{
    615 int     depth=0;
    616 if(node) getmaxdepth(node,1,depth);
    617 return(depth);
     615        int     depth=0;
     616        if(node) getmaxdepth(node,1,depth);
     617        return(depth);
    618618}
    619619
    … …  
    621621int                             btDbvt::countLeaves(const btDbvtNode* node)
    622622{
    623 if(node->isinternal())
    624         return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
     623        if(node->isinternal())
     624                return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
    625625        else
    626         return(1);
     626                return(1);
    627627}
    628628
    … …  
    630630void                    btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
    631631{
    632 if(node->isinternal())
    633         {
    634         extractLeaves(node->childs[0],leaves);
    635         extractLeaves(node->childs[1],leaves);
     632        if(node->isinternal())
     633        {
     634                extractLeaves(node->childs[0],leaves);
     635                extractLeaves(node->childs[1],leaves);
    636636        }
    637637        else
    638638        {
    639         leaves.push_back(node);
     639                leaves.push_back(node);
    640640        }       
    641641}
    … …  
    658658
    659659Benchmarking dbvt...
    660         World scale: 100.000000
    661         Extents base: 1.000000
    662         Extents range: 4.000000
    663         Leaves: 8192
    664         sizeof(btDbvtVolume): 32 bytes
    665         sizeof(btDbvtNode):   44 bytes
     660World scale: 100.000000
     661Extents base: 1.000000
     662Extents range: 4.000000
     663Leaves: 8192
     664sizeof(btDbvtVolume): 32 bytes
     665sizeof(btDbvtNode):   44 bytes
    666666[1] btDbvtVolume intersections: 3499 ms (-1%)
    667667[2] btDbvtVolume merges: 1934 ms (0%)
    … …  
    670670[5] btDbvt::collideTT xform: 7379 ms (-1%)
    671671[6] btDbvt::collideTT xform,self: 7270 ms (-2%)
    672 [7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s)
     672[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s)
    673673[8] insert/remove: 2093 ms (0%),(1001983 ir/s)
    674674[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
    … …  
    685685struct btDbvtBenchmark
    686686{
    687 struct NilPolicy : btDbvt::ICollide
    688         {
    689         NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true)             {}
    690         void    Process(const btDbvtNode*,const btDbvtNode*)                            { ++m_pcount; }
    691         void    Process(const btDbvtNode*)                                                                      { ++m_pcount; }
    692         void    Process(const btDbvtNode*,btScalar depth)
    693                 {
    694                 ++m_pcount;
    695                 if(m_checksort)
     687        struct NilPolicy : btDbvt::ICollide
     688        {
     689                NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true)             {}
     690                void    Process(const btDbvtNode*,const btDbvtNode*)                            { ++m_pcount; }
     691                void    Process(const btDbvtNode*)                                                                      { ++m_pcount; }
     692                void    Process(const btDbvtNode*,btScalar depth)
     693                {
     694                        ++m_pcount;
     695                        if(m_checksort)
    696696                        { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
    697697                }
    698         int                     m_pcount;
    699         btScalar        m_depth;
    700         bool            m_checksort;
     698                int                     m_pcount;
     699                btScalar        m_depth;
     700                bool            m_checksort;
    701701        };
    702 struct P14 : btDbvt::ICollide
    703         {
    704         struct Node
    705                 {
    706                 const btDbvtNode*       leaf;
    707                 btScalar                        depth;
     702        struct P14 : btDbvt::ICollide
     703        {
     704                struct Node
     705                {
     706                        const btDbvtNode*       leaf;
     707                        btScalar                        depth;
    708708                };
    709         void Process(const btDbvtNode* leaf,btScalar depth)
    710                 {
    711                 Node    n;
    712                 n.leaf  =       leaf;
    713                 n.depth =       depth;
    714                 }
    715         static int sortfnc(const Node& a,const Node& b)
    716                 {
    717                 if(a.depth<b.depth) return(+1);
    718                 if(a.depth>b.depth) return(-1);
    719                 return(0);
    720                 }
    721         btAlignedObjectArray<Node>              m_nodes;
     709                void Process(const btDbvtNode* leaf,btScalar depth)
     710                {
     711                        Node    n;
     712                        n.leaf  =       leaf;
     713                        n.depth =       depth;
     714                }
     715                static int sortfnc(const Node& a,const Node& b)
     716                {
     717                        if(a.depth<b.depth) return(+1);
     718                        if(a.depth>b.depth) return(-1);
     719                        return(0);
     720                }
     721                btAlignedObjectArray<Node>              m_nodes;
    722722        };
    723 struct P15 : btDbvt::ICollide
    724         {
    725         struct Node
    726                 {
    727                 const btDbvtNode*       leaf;
    728                 btScalar                        depth;
     723        struct P15 : btDbvt::ICollide
     724        {
     725                struct Node
     726                {
     727                        const btDbvtNode*       leaf;
     728                        btScalar                        depth;
    729729                };
    730         void Process(const btDbvtNode* leaf)
    731                 {
    732                 Node    n;
    733                 n.leaf  =       leaf;
    734                 n.depth =       dot(leaf->volume.Center(),m_axis);
    735                 }
    736         static int sortfnc(const Node& a,const Node& b)
    737                 {
    738                 if(a.depth<b.depth) return(+1);
    739                 if(a.depth>b.depth) return(-1);
    740                 return(0);
    741                 }
    742         btAlignedObjectArray<Node>              m_nodes;
    743         btVector3                                               m_axis;
     730                void Process(const btDbvtNode* leaf)
     731                {
     732                        Node    n;
     733                        n.leaf  =       leaf;
     734                        n.depth =       dot(leaf->volume.Center(),m_axis);
     735                }
     736                static int sortfnc(const Node& a,const Node& b)
     737                {
     738                        if(a.depth<b.depth) return(+1);
     739                        if(a.depth>b.depth) return(-1);
     740                        return(0);
     741                }
     742                btAlignedObjectArray<Node>              m_nodes;
     743                btVector3                                               m_axis;
    744744        };
    745 static btScalar                 RandUnit()
    746         {
    747         return(rand()/(btScalar)RAND_MAX);
    748         }
    749 static btVector3                RandVector3()
    750         {
    751         return(btVector3(RandUnit(),RandUnit(),RandUnit()));
    752         }
    753 static btVector3                RandVector3(btScalar cs)
    754         {
    755         return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
    756         }
    757 static btDbvtVolume     RandVolume(btScalar cs,btScalar eb,btScalar es)
    758         {
    759         return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
    760         }
    761 static btTransform              RandTransform(btScalar cs)
    762         {
    763         btTransform     t;
    764         t.setOrigin(RandVector3(cs));
    765         t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
    766         return(t);
    767         }
    768 static void                             RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
    769         {
    770         dbvt.clear();
    771         for(int i=0;i<leaves;++i)
    772                 {
    773                 dbvt.insert(RandVolume(cs,eb,es),0);
     745        static btScalar                 RandUnit()
     746        {
     747                return(rand()/(btScalar)RAND_MAX);
     748        }
     749        static btVector3                RandVector3()
     750        {
     751                return(btVector3(RandUnit(),RandUnit(),RandUnit()));
     752        }
     753        static btVector3                RandVector3(btScalar cs)
     754        {
     755                return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
     756        }
     757        static btDbvtVolume     RandVolume(btScalar cs,btScalar eb,btScalar es)
     758        {
     759                return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
     760        }
     761        static btTransform              RandTransform(btScalar cs)
     762        {
     763                btTransform     t;
     764                t.setOrigin(RandVector3(cs));
     765                t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
     766                return(t);
     767        }
     768        static void                             RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
     769        {
     770                dbvt.clear();
     771                for(int i=0;i<leaves;++i)
     772                {
     773                        dbvt.insert(RandVolume(cs,eb,es),0);
    774774                }
    775775        }
    … …  
    778778void                    btDbvt::benchmark()
    779779{
    780 static const btScalar   cfgVolumeCenterScale            =       100;
    781 static const btScalar   cfgVolumeExentsBase                     =       1;
    782 static const btScalar   cfgVolumeExentsScale            =       4;
    783 static const int                cfgLeaves                                       =       8192;
    784 static const bool               cfgEnable                                       =       true;
    785 
    786 //[1] btDbvtVolume intersections
    787 bool                                    cfgBenchmark1_Enable            =       cfgEnable;
    788 static const int                cfgBenchmark1_Iterations        =       8;
    789 static const int                cfgBenchmark1_Reference         =       3499;
    790 //[2] btDbvtVolume merges
    791 bool                                    cfgBenchmark2_Enable            =       cfgEnable;
    792 static const int                cfgBenchmark2_Iterations        =       4;
    793 static const int                cfgBenchmark2_Reference         =       1945;
    794 //[3] btDbvt::collideTT
    795 bool                                    cfgBenchmark3_Enable            =       cfgEnable;
    796 static const int                cfgBenchmark3_Iterations        =       512;
    797 static const int                cfgBenchmark3_Reference         =       5485;
    798 //[4] btDbvt::collideTT self
    799 bool                                    cfgBenchmark4_Enable            =       cfgEnable;
    800 static const int                cfgBenchmark4_Iterations        =       512;
    801 static const int                cfgBenchmark4_Reference         =       2814;
    802 //[5] btDbvt::collideTT xform
    803 bool                                    cfgBenchmark5_Enable            =       cfgEnable;
    804 static const int                cfgBenchmark5_Iterations        =       512;
    805 static const btScalar   cfgBenchmark5_OffsetScale       =       2;
    806 static const int                cfgBenchmark5_Reference         =       7379;
    807 //[6] btDbvt::collideTT xform,self
    808 bool                                    cfgBenchmark6_Enable            =       cfgEnable;
    809 static const int                cfgBenchmark6_Iterations        =       512;
    810 static const btScalar   cfgBenchmark6_OffsetScale       =       2;
    811 static const int                cfgBenchmark6_Reference         =       7270;
    812 //[7] btDbvt::collideRAY
    813 bool                                    cfgBenchmark7_Enable            =       cfgEnable;
    814 static const int                cfgBenchmark7_Passes            =       32;
    815 static const int                cfgBenchmark7_Iterations        =       65536;
    816 static const int                cfgBenchmark7_Reference         =       6307;
    817 //[8] insert/remove
    818 bool                                    cfgBenchmark8_Enable            =       cfgEnable;
    819 static const int                cfgBenchmark8_Passes            =       32;
    820 static const int                cfgBenchmark8_Iterations        =       65536;
    821 static const int                cfgBenchmark8_Reference         =       2105;
    822 //[9] updates (teleport)
    823 bool                                    cfgBenchmark9_Enable            =       cfgEnable;
    824 static const int                cfgBenchmark9_Passes            =       32;
    825 static const int                cfgBenchmark9_Iterations        =       65536;
    826 static const int                cfgBenchmark9_Reference         =       1879;
    827 //[10] updates (jitter)
    828 bool                                    cfgBenchmark10_Enable           =       cfgEnable;
    829 static const btScalar   cfgBenchmark10_Scale            =       cfgVolumeCenterScale/10000;
    830 static const int                cfgBenchmark10_Passes           =       32;
    831 static const int                cfgBenchmark10_Iterations       =       65536;
    832 static const int                cfgBenchmark10_Reference        =       1244;
    833 //[11] optimize (incremental)
    834 bool                                    cfgBenchmark11_Enable           =       cfgEnable;
    835 static const int                cfgBenchmark11_Passes           =       64;
    836 static const int                cfgBenchmark11_Iterations       =       65536;
    837 static const int                cfgBenchmark11_Reference        =       2510;
    838 //[12] btDbvtVolume notequal
    839 bool                                    cfgBenchmark12_Enable           =       cfgEnable;
    840 static const int                cfgBenchmark12_Iterations       =       32;
    841 static const int                cfgBenchmark12_Reference        =       3677;
    842 //[13] culling(OCL+fullsort)
    843 bool                                    cfgBenchmark13_Enable           =       cfgEnable;
    844 static const int                cfgBenchmark13_Iterations       =       1024;
    845 static const int                cfgBenchmark13_Reference        =       2231;
    846 //[14] culling(OCL+qsort)
    847 bool                                    cfgBenchmark14_Enable           =       cfgEnable;
    848 static const int                cfgBenchmark14_Iterations       =       8192;
    849 static const int                cfgBenchmark14_Reference        =       3500;
    850 //[15] culling(KDOP+qsort)
    851 bool                                    cfgBenchmark15_Enable           =       cfgEnable;
    852 static const int                cfgBenchmark15_Iterations       =       8192;
    853 static const int                cfgBenchmark15_Reference        =       1151;
    854 //[16] insert/remove batch
    855 bool                                    cfgBenchmark16_Enable           =       cfgEnable;
    856 static const int                cfgBenchmark16_BatchCount       =       256;
    857 static const int                cfgBenchmark16_Passes           =       16384;
    858 static const int                cfgBenchmark16_Reference        =       5138;
    859 //[17] select
    860 bool                                    cfgBenchmark17_Enable           =       cfgEnable;
    861 static const int                cfgBenchmark17_Iterations       =       4;
    862 static const int                cfgBenchmark17_Reference        =       3390;
    863 
    864 btClock                                 wallclock;
    865 printf("Benchmarking dbvt...\r\n");
    866 printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
    867 printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
    868 printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
    869 printf("\tLeaves: %u\r\n",cfgLeaves);
    870 printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
    871 printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
    872 if(cfgBenchmark1_Enable)
     780        static const btScalar   cfgVolumeCenterScale            =       100;
     781        static const btScalar   cfgVolumeExentsBase                     =       1;
     782        static const btScalar   cfgVolumeExentsScale            =       4;
     783        static const int                cfgLeaves                                       =       8192;
     784        static const bool               cfgEnable                                       =       true;
     785
     786        //[1] btDbvtVolume intersections
     787        bool                                    cfgBenchmark1_Enable            =       cfgEnable;
     788        static const int                cfgBenchmark1_Iterations        =       8;
     789        static const int                cfgBenchmark1_Reference         =       3499;
     790        //[2] btDbvtVolume merges
     791        bool                                    cfgBenchmark2_Enable            =       cfgEnable;
     792        static const int                cfgBenchmark2_Iterations        =       4;
     793        static const int                cfgBenchmark2_Reference         =       1945;
     794        //[3] btDbvt::collideTT
     795        bool                                    cfgBenchmark3_Enable            =       cfgEnable;
     796        static const int                cfgBenchmark3_Iterations        =       512;
     797        static const int                cfgBenchmark3_Reference         =       5485;
     798        //[4] btDbvt::collideTT self
     799        bool                                    cfgBenchmark4_Enable            =       cfgEnable;
     800        static const int                cfgBenchmark4_Iterations        =       512;
     801        static const int                cfgBenchmark4_Reference         =       2814;
     802        //[5] btDbvt::collideTT xform
     803        bool                                    cfgBenchmark5_Enable            =       cfgEnable;
     804        static const int                cfgBenchmark5_Iterations        =       512;
     805        static const btScalar   cfgBenchmark5_OffsetScale       =       2;
     806        static const int                cfgBenchmark5_Reference         =       7379;
     807        //[6] btDbvt::collideTT xform,self
     808        bool                                    cfgBenchmark6_Enable            =       cfgEnable;
     809        static const int                cfgBenchmark6_Iterations        =       512;
     810        static const btScalar   cfgBenchmark6_OffsetScale       =       2;
     811        static const int                cfgBenchmark6_Reference         =       7270;
     812        //[7] btDbvt::rayTest
     813        bool                                    cfgBenchmark7_Enable            =       cfgEnable;
     814        static const int                cfgBenchmark7_Passes            =       32;
     815        static const int                cfgBenchmark7_Iterations        =       65536;
     816        static const int                cfgBenchmark7_Reference         =       6307;
     817        //[8] insert/remove
     818        bool                                    cfgBenchmark8_Enable            =       cfgEnable;
     819        static const int                cfgBenchmark8_Passes            =       32;
     820        static const int                cfgBenchmark8_Iterations        =       65536;
     821        static const int                cfgBenchmark8_Reference         =       2105;
     822        //[9] updates (teleport)
     823        bool                                    cfgBenchmark9_Enable            =       cfgEnable;
     824        static const int                cfgBenchmark9_Passes            =       32;
     825        static const int                cfgBenchmark9_Iterations        =       65536;
     826        static const int                cfgBenchmark9_Reference         =       1879;
     827        //[10] updates (jitter)
     828        bool                                    cfgBenchmark10_Enable           =       cfgEnable;
     829        static const btScalar   cfgBenchmark10_Scale            =       cfgVolumeCenterScale/10000;
     830        static const int                cfgBenchmark10_Passes           =       32;
     831        static const int                cfgBenchmark10_Iterations       =       65536;
     832        static const int                cfgBenchmark10_Reference        =       1244;
     833        //[11] optimize (incremental)
     834        bool                                    cfgBenchmark11_Enable           =       cfgEnable;
     835        static const int                cfgBenchmark11_Passes           =       64;
     836        static const int                cfgBenchmark11_Iterations       =       65536;
     837        static const int                cfgBenchmark11_Reference        =       2510;
     838        //[12] btDbvtVolume notequal
     839        bool                                    cfgBenchmark12_Enable           =       cfgEnable;
     840        static const int                cfgBenchmark12_Iterations       =       32;
     841        static const int                cfgBenchmark12_Reference        =       3677;
     842        //[13] culling(OCL+fullsort)
     843        bool                                    cfgBenchmark13_Enable           =       cfgEnable;
     844        static const int                cfgBenchmark13_Iterations       =       1024;
     845        static const int                cfgBenchmark13_Reference        =       2231;
     846        //[14] culling(OCL+qsort)
     847        bool                                    cfgBenchmark14_Enable           =       cfgEnable;
     848        static const int                cfgBenchmark14_Iterations       =       8192;
     849        static const int                cfgBenchmark14_Reference        =       3500;
     850        //[15] culling(KDOP+qsort)
     851        bool                                    cfgBenchmark15_Enable           =       cfgEnable;
     852        static const int                cfgBenchmark15_Iterations       =       8192;
     853        static const int                cfgBenchmark15_Reference        =       1151;
     854        //[16] insert/remove batch
     855        bool                                    cfgBenchmark16_Enable           =       cfgEnable;
     856        static const int                cfgBenchmark16_BatchCount       =       256;
     857        static const int                cfgBenchmark16_Passes           =       16384;
     858        static const int                cfgBenchmark16_Reference        =       5138;
     859        //[17] select
     860        bool                                    cfgBenchmark17_Enable           =       cfgEnable;
     861        static const int                cfgBenchmark17_Iterations       =       4;
     862        static const int                cfgBenchmark17_Reference        =       3390;
     863
     864        btClock                                 wallclock;
     865        printf("Benchmarking dbvt...\r\n");
     866        printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
     867        printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
     868        printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
     869        printf("\tLeaves: %u\r\n",cfgLeaves);
     870        printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
     871        printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
     872        if(cfgBenchmark1_Enable)
    873873        {// Benchmark 1
    874         srand(380843);
    875         btAlignedObjectArray<btDbvtVolume>      volumes;
    876         btAlignedObjectArray<bool>                      results;
    877         volumes.resize(cfgLeaves);
    878         results.resize(cfgLeaves);
    879         for(int i=0;i<cfgLeaves;++i)
    880                 {
    881                 volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
    882                 }
    883         printf("[1] btDbvtVolume intersections: ");
    884         wallclock.reset();
    885         for(int i=0;i<cfgBenchmark1_Iterations;++i)
    886                 {
    887                 for(int j=0;j<cfgLeaves;++j)
    888                         {
    889                         for(int k=0;k<cfgLeaves;++k)
     874                srand(380843);
     875                btAlignedObjectArray<btDbvtVolume>      volumes;
     876                btAlignedObjectArray<bool>                      results;
     877                volumes.resize(cfgLeaves);
     878                results.resize(cfgLeaves);
     879                for(int i=0;i<cfgLeaves;++i)
     880                {
     881                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
     882                }
     883                printf("[1] btDbvtVolume intersections: ");
     884                wallclock.reset();
     885                for(int i=0;i<cfgBenchmark1_Iterations;++i)
     886                {
     887                        for(int j=0;j<cfgLeaves;++j)
     888                        {
     889                                for(int k=0;k<cfgLeaves;++k)
    890890                                {
    891                                 results[k]=Intersect(volumes[j],volumes[k]);
     891                                        results[k]=Intersect(volumes[j],volumes[k]);
    892892                                }
    893893                        }
    894894                }
    895         const int time=(int)wallclock.getTimeMilliseconds();
    896         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
    897         }
    898 if(cfgBenchmark2_Enable)
     895                const int time=(int)wallclock.getTimeMilliseconds();
     896                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
     897        }
     898        if(cfgBenchmark2_Enable)
    899899        {// Benchmark 2
    900         srand(380843);
    901         btAlignedObjectArray<btDbvtVolume>      volumes;
    902         btAlignedObjectArray<btDbvtVolume>      results;
    903         volumes.resize(cfgLeaves);
    904         results.resize(cfgLeaves);
    905         for(int i=0;i<cfgLeaves;++i)
    906                 {
    907                 volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
    908                 }
    909         printf("[2] btDbvtVolume merges: ");
    910         wallclock.reset();
    911         for(int i=0;i<cfgBenchmark2_Iterations;++i)
    912                 {
    913                 for(int j=0;j<cfgLeaves;++j)
    914                         {
    915                         for(int k=0;k<cfgLeaves;++k)
     900                srand(380843);
     901                btAlignedObjectArray<btDbvtVolume>      volumes;
     902                btAlignedObjectArray<btDbvtVolume>      results;
     903                volumes.resize(cfgLeaves);
     904                results.resize(cfgLeaves);
     905                for(int i=0;i<cfgLeaves;++i)
     906                {
     907                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
     908                }
     909                printf("[2] btDbvtVolume merges: ");
     910                wallclock.reset();
     911                for(int i=0;i<cfgBenchmark2_Iterations;++i)
     912                {
     913                        for(int j=0;j<cfgLeaves;++j)
     914                        {
     915                                for(int k=0;k<cfgLeaves;++k)
    916916                                {
    917                                 Merge(volumes[j],volumes[k],results[k]);
     917                                        Merge(volumes[j],volumes[k],results[k]);
    918918                                }
    919919                        }
    920920                }
    921         const int time=(int)wallclock.getTimeMilliseconds();
    922         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
    923         }
    924 if(cfgBenchmark3_Enable)
     921                const int time=(int)wallclock.getTimeMilliseconds();
     922                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
     923        }
     924        if(cfgBenchmark3_Enable)
    925925        {// Benchmark 3
    926         srand(380843);
    927         btDbvt                                          dbvt[2];
    928         btDbvtBenchmark::NilPolicy      policy;
    929         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
    930         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
    931         dbvt[0].optimizeTopDown();
    932         dbvt[1].optimizeTopDown();
    933         printf("[3] btDbvt::collideTT: ");
    934         wallclock.reset();
    935         for(int i=0;i<cfgBenchmark3_Iterations;++i)
    936                 {
    937                 btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
    938                 }
    939         const int time=(int)wallclock.getTimeMilliseconds();
    940         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
    941         }
    942 if(cfgBenchmark4_Enable)
     926                srand(380843);
     927                btDbvt                                          dbvt[2];
     928                btDbvtBenchmark::NilPolicy      policy;
     929                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
     930                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
     931                dbvt[0].optimizeTopDown();
     932                dbvt[1].optimizeTopDown();
     933                printf("[3] btDbvt::collideTT: ");
     934                wallclock.reset();
     935                for(int i=0;i<cfgBenchmark3_Iterations;++i)
     936                {
     937                        btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
     938                }
     939                const int time=(int)wallclock.getTimeMilliseconds();
     940                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
     941        }
     942        if(cfgBenchmark4_Enable)
    943943        {// Benchmark 4
    944         srand(380843);
    945         btDbvt                                          dbvt;
    946         btDbvtBenchmark::NilPolicy      policy;
    947         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    948         dbvt.optimizeTopDown();
    949         printf("[4] btDbvt::collideTT self: ");
    950         wallclock.reset();
    951         for(int i=0;i<cfgBenchmark4_Iterations;++i)
    952                 {
    953                 btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
    954                 }
    955         const int time=(int)wallclock.getTimeMilliseconds();
    956         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
    957         }
    958 if(cfgBenchmark5_Enable)
     944                srand(380843);
     945                btDbvt                                          dbvt;
     946                btDbvtBenchmark::NilPolicy      policy;
     947                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     948                dbvt.optimizeTopDown();
     949                printf("[4] btDbvt::collideTT self: ");
     950                wallclock.reset();
     951                for(int i=0;i<cfgBenchmark4_Iterations;++i)
     952                {
     953                        btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
     954                }
     955                const int time=(int)wallclock.getTimeMilliseconds();
     956                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
     957        }
     958        if(cfgBenchmark5_Enable)
    959959        {// Benchmark 5
    960         srand(380843);
    961         btDbvt                                                          dbvt[2];
    962         btAlignedObjectArray<btTransform>       transforms;
    963         btDbvtBenchmark::NilPolicy                      policy;
    964         transforms.resize(cfgBenchmark5_Iterations);
    965         for(int i=0;i<transforms.size();++i)
    966                 {
    967                 transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
    968                 }
    969         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
    970         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
    971         dbvt[0].optimizeTopDown();
    972         dbvt[1].optimizeTopDown();
    973         printf("[5] btDbvt::collideTT xform: ");
    974         wallclock.reset();
    975         for(int i=0;i<cfgBenchmark5_Iterations;++i)
    976                 {
    977                 btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
    978                 }
    979         const int time=(int)wallclock.getTimeMilliseconds();
    980         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
    981         }
    982 if(cfgBenchmark6_Enable)
     960                srand(380843);
     961                btDbvt                                                          dbvt[2];
     962                btAlignedObjectArray<btTransform>       transforms;
     963                btDbvtBenchmark::NilPolicy                      policy;
     964                transforms.resize(cfgBenchmark5_Iterations);
     965                for(int i=0;i<transforms.size();++i)
     966                {
     967                        transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
     968                }
     969                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
     970                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
     971                dbvt[0].optimizeTopDown();
     972                dbvt[1].optimizeTopDown();
     973                printf("[5] btDbvt::collideTT xform: ");
     974                wallclock.reset();
     975                for(int i=0;i<cfgBenchmark5_Iterations;++i)
     976                {
     977                        btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
     978                }
     979                const int time=(int)wallclock.getTimeMilliseconds();
     980                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
     981        }
     982        if(cfgBenchmark6_Enable)
    983983        {// Benchmark 6
    984         srand(380843);
    985         btDbvt                                                          dbvt;
    986         btAlignedObjectArray<btTransform>       transforms;
    987         btDbvtBenchmark::NilPolicy                      policy;
    988         transforms.resize(cfgBenchmark6_Iterations);
    989         for(int i=0;i<transforms.size();++i)
    990                 {
    991                 transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
    992                 }
    993         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    994         dbvt.optimizeTopDown();
    995         printf("[6] btDbvt::collideTT xform,self: ");
    996         wallclock.reset();
    997         for(int i=0;i<cfgBenchmark6_Iterations;++i)
    998                 {
    999                 btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);               
    1000                 }
    1001         const int time=(int)wallclock.getTimeMilliseconds();
    1002         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
    1003         }
    1004 if(cfgBenchmark7_Enable)
     984                srand(380843);
     985                btDbvt                                                          dbvt;
     986                btAlignedObjectArray<btTransform>       transforms;
     987                btDbvtBenchmark::NilPolicy                      policy;
     988                transforms.resize(cfgBenchmark6_Iterations);
     989                for(int i=0;i<transforms.size();++i)
     990                {
     991                        transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
     992                }
     993                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     994                dbvt.optimizeTopDown();
     995                printf("[6] btDbvt::collideTT xform,self: ");
     996                wallclock.reset();
     997                for(int i=0;i<cfgBenchmark6_Iterations;++i)
     998                {
     999                        btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);               
     1000                }
     1001                const int time=(int)wallclock.getTimeMilliseconds();
     1002                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
     1003        }
     1004        if(cfgBenchmark7_Enable)
    10051005        {// Benchmark 7
    1006         srand(380843);
    1007         btDbvt                                                          dbvt;
    1008         btAlignedObjectArray<btVector3>         rayorg;
    1009         btAlignedObjectArray<btVector3>         raydir;
    1010         btDbvtBenchmark::NilPolicy                      policy;
    1011         rayorg.resize(cfgBenchmark7_Iterations);
    1012         raydir.resize(cfgBenchmark7_Iterations);
    1013         for(int i=0;i<rayorg.size();++i)
    1014                 {
    1015                 rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
    1016                 raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
    1017                 }
    1018         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1019         dbvt.optimizeTopDown();
    1020         printf("[7] btDbvt::collideRAY: ");
    1021         wallclock.reset();
    1022         for(int i=0;i<cfgBenchmark7_Passes;++i)
    1023                 {
    1024                 for(int j=0;j<cfgBenchmark7_Iterations;++j)
    1025                         {
    1026                         btDbvt::collideRAY(dbvt.m_root,rayorg[j],raydir[j],policy);
    1027                         }
    1028                 }
    1029         const int       time=(int)wallclock.getTimeMilliseconds();
    1030         unsigned        rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
    1031         printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
    1032         }
    1033 if(cfgBenchmark8_Enable)
     1006                srand(380843);
     1007                btDbvt                                                          dbvt;
     1008                btAlignedObjectArray<btVector3>         rayorg;
     1009                btAlignedObjectArray<btVector3>         raydir;
     1010                btDbvtBenchmark::NilPolicy                      policy;
     1011                rayorg.resize(cfgBenchmark7_Iterations);
     1012                raydir.resize(cfgBenchmark7_Iterations);
     1013                for(int i=0;i<rayorg.size();++i)
     1014                {
     1015                        rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
     1016                        raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
     1017                }
     1018                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1019                dbvt.optimizeTopDown();
     1020                printf("[7] btDbvt::rayTest: ");
     1021                wallclock.reset();
     1022                for(int i=0;i<cfgBenchmark7_Passes;++i)
     1023                {
     1024                        for(int j=0;j<cfgBenchmark7_Iterations;++j)
     1025                        {
     1026                                btDbvt::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy);
     1027                        }
     1028                }
     1029                const int       time=(int)wallclock.getTimeMilliseconds();
     1030                unsigned        rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
     1031                printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
     1032        }
     1033        if(cfgBenchmark8_Enable)
    10341034        {// Benchmark 8
    1035         srand(380843);
    1036         btDbvt                                                          dbvt;
    1037         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1038         dbvt.optimizeTopDown();
    1039         printf("[8] insert/remove: ");
    1040         wallclock.reset();
    1041         for(int i=0;i<cfgBenchmark8_Passes;++i)
    1042                 {
    1043                 for(int j=0;j<cfgBenchmark8_Iterations;++j)
    1044                         {
    1045                         dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
    1046                         }
    1047                 }
    1048         const int       time=(int)wallclock.getTimeMilliseconds();
    1049         const int       ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
    1050         printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
    1051         }
    1052 if(cfgBenchmark9_Enable)
     1035                srand(380843);
     1036                btDbvt                                                          dbvt;
     1037                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1038                dbvt.optimizeTopDown();
     1039                printf("[8] insert/remove: ");
     1040                wallclock.reset();
     1041                for(int i=0;i<cfgBenchmark8_Passes;++i)
     1042                {
     1043                        for(int j=0;j<cfgBenchmark8_Iterations;++j)
     1044                        {
     1045                                dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
     1046                        }
     1047                }
     1048                const int       time=(int)wallclock.getTimeMilliseconds();
     1049                const int       ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
     1050                printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
     1051        }
     1052        if(cfgBenchmark9_Enable)
    10531053        {// Benchmark 9
    1054         srand(380843);
    1055         btDbvt                                                                          dbvt;
    1056         btAlignedObjectArray<const btDbvtNode*> leaves;
    1057         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1058         dbvt.optimizeTopDown();
    1059         dbvt.extractLeaves(dbvt.m_root,leaves);
    1060         printf("[9] updates (teleport): ");
    1061         wallclock.reset();
    1062         for(int i=0;i<cfgBenchmark9_Passes;++i)
    1063                 {
    1064                 for(int j=0;j<cfgBenchmark9_Iterations;++j)
    1065                         {
    1066                         dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
    1067                                                 btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
    1068                         }
    1069                 }
    1070         const int       time=(int)wallclock.getTimeMilliseconds();
    1071         const int       up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
    1072         printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
    1073         }
    1074 if(cfgBenchmark10_Enable)
     1054                srand(380843);
     1055                btDbvt                                                                          dbvt;
     1056                btAlignedObjectArray<const btDbvtNode*> leaves;
     1057                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1058                dbvt.optimizeTopDown();
     1059                dbvt.extractLeaves(dbvt.m_root,leaves);
     1060                printf("[9] updates (teleport): ");
     1061                wallclock.reset();
     1062                for(int i=0;i<cfgBenchmark9_Passes;++i)
     1063                {
     1064                        for(int j=0;j<cfgBenchmark9_Iterations;++j)
     1065                        {
     1066                                dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
     1067                                        btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
     1068                        }
     1069                }
     1070                const int       time=(int)wallclock.getTimeMilliseconds();
     1071                const int       up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
     1072                printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
     1073        }
     1074        if(cfgBenchmark10_Enable)
    10751075        {// Benchmark 10       
    1076         srand(380843);
    1077         btDbvt                                                                          dbvt;
    1078         btAlignedObjectArray<const btDbvtNode*> leaves;
    1079         btAlignedObjectArray<btVector3>                         vectors;
    1080         vectors.resize(cfgBenchmark10_Iterations);
    1081         for(int i=0;i<vectors.size();++i)
    1082                 {
    1083                 vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
    1084                 }
    1085         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1086         dbvt.optimizeTopDown();
    1087         dbvt.extractLeaves(dbvt.m_root,leaves);
    1088         printf("[10] updates (jitter): ");
    1089         wallclock.reset();
    1090        
    1091         for(int i=0;i<cfgBenchmark10_Passes;++i)
    1092                 {
    1093                 for(int j=0;j<cfgBenchmark10_Iterations;++j)
     1076                srand(380843);
     1077                btDbvt                                                                          dbvt;
     1078                btAlignedObjectArray<const btDbvtNode*> leaves;
     1079                btAlignedObjectArray<btVector3>                         vectors;
     1080                vectors.resize(cfgBenchmark10_Iterations);
     1081                for(int i=0;i<vectors.size();++i)
     1082                {
     1083                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
     1084                }
     1085                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1086                dbvt.optimizeTopDown();
     1087                dbvt.extractLeaves(dbvt.m_root,leaves);
     1088                printf("[10] updates (jitter): ");
     1089                wallclock.reset();
     1090
     1091                for(int i=0;i<cfgBenchmark10_Passes;++i)
     1092                {
     1093                        for(int j=0;j<cfgBenchmark10_Iterations;++j)
    10941094                        {                       
    1095                         const btVector3&        d=vectors[j];
    1096                         btDbvtNode*             l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
    1097                         btDbvtVolume            v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
    1098                         dbvt.update(l,v);
    1099                         }
    1100                 }
    1101         const int       time=(int)wallclock.getTimeMilliseconds();
    1102         const int       up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
    1103         printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
    1104         }
    1105 if(cfgBenchmark11_Enable)
     1095                                const btVector3&        d=vectors[j];
     1096                                btDbvtNode*             l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
     1097                                btDbvtVolume            v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
     1098                                dbvt.update(l,v);
     1099                        }
     1100                }
     1101                const int       time=(int)wallclock.getTimeMilliseconds();
     1102                const int       up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
     1103                printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
     1104        }
     1105        if(cfgBenchmark11_Enable)
    11061106        {// Benchmark 11       
    1107         srand(380843);
    1108         btDbvt                                                                          dbvt;
    1109         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1110         dbvt.optimizeTopDown();
    1111         printf("[11] optimize (incremental): ");
    1112         wallclock.reset();     
    1113         for(int i=0;i<cfgBenchmark11_Passes;++i)
    1114                 {
    1115                 dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
    1116                 }
    1117         const int       time=(int)wallclock.getTimeMilliseconds();
    1118         const int       op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
    1119         printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
    1120         }
    1121 if(cfgBenchmark12_Enable)
     1107                srand(380843);
     1108                btDbvt                                                                          dbvt;
     1109                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1110                dbvt.optimizeTopDown();
     1111                printf("[11] optimize (incremental): ");
     1112                wallclock.reset();     
     1113                for(int i=0;i<cfgBenchmark11_Passes;++i)
     1114                {
     1115                        dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
     1116                }
     1117                const int       time=(int)wallclock.getTimeMilliseconds();
     1118                const int       op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
     1119                printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
     1120        }
     1121        if(cfgBenchmark12_Enable)
    11221122        {// Benchmark 12       
    1123         srand(380843);
    1124         btAlignedObjectArray<btDbvtVolume>      volumes;
    1125         btAlignedObjectArray<bool>                              results;
    1126         volumes.resize(cfgLeaves);
    1127         results.resize(cfgLeaves);
    1128         for(int i=0;i<cfgLeaves;++i)
    1129                 {
    1130                 volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
    1131                 }
    1132         printf("[12] btDbvtVolume notequal: ");
    1133         wallclock.reset();
    1134         for(int i=0;i<cfgBenchmark12_Iterations;++i)
    1135                 {
    1136                 for(int j=0;j<cfgLeaves;++j)
    1137                         {
    1138                         for(int k=0;k<cfgLeaves;++k)
     1123                srand(380843);
     1124                btAlignedObjectArray<btDbvtVolume>      volumes;
     1125                btAlignedObjectArray<bool>                              results;
     1126                volumes.resize(cfgLeaves);
     1127                results.resize(cfgLeaves);
     1128                for(int i=0;i<cfgLeaves;++i)
     1129                {
     1130                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
     1131                }
     1132                printf("[12] btDbvtVolume notequal: ");
     1133                wallclock.reset();
     1134                for(int i=0;i<cfgBenchmark12_Iterations;++i)
     1135                {
     1136                        for(int j=0;j<cfgLeaves;++j)
     1137                        {
     1138                                for(int k=0;k<cfgLeaves;++k)
    11391139                                {
    1140                                 results[k]=NotEqual(volumes[j],volumes[k]);
     1140                                        results[k]=NotEqual(volumes[j],volumes[k]);
    11411141                                }
    11421142                        }
    11431143                }
    1144         const int time=(int)wallclock.getTimeMilliseconds();
    1145         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
    1146         }
    1147 if(cfgBenchmark13_Enable)
     1144                const int time=(int)wallclock.getTimeMilliseconds();
     1145                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
     1146        }
     1147        if(cfgBenchmark13_Enable)
    11481148        {// Benchmark 13       
    1149         srand(380843);
    1150         btDbvt                                                          dbvt;
    1151         btAlignedObjectArray<btVector3>         vectors;
    1152         btDbvtBenchmark::NilPolicy                      policy;
    1153         vectors.resize(cfgBenchmark13_Iterations);
    1154         for(int i=0;i<vectors.size();++i)
    1155                 {
    1156                 vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
    1157                 }
    1158         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1159         dbvt.optimizeTopDown();
    1160         printf("[13] culling(OCL+fullsort): ");
    1161         wallclock.reset();     
    1162         for(int i=0;i<cfgBenchmark13_Iterations;++i)
    1163                 {
    1164                 static const btScalar   offset=0;
    1165                 policy.m_depth=-SIMD_INFINITY;
    1166                 dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
    1167                 }
    1168         const int       time=(int)wallclock.getTimeMilliseconds();
    1169         const int       t=cfgBenchmark13_Iterations;
    1170         printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
    1171         }
    1172 if(cfgBenchmark14_Enable)
     1149                srand(380843);
     1150                btDbvt                                                          dbvt;
     1151                btAlignedObjectArray<btVector3>         vectors;
     1152                btDbvtBenchmark::NilPolicy                      policy;
     1153                vectors.resize(cfgBenchmark13_Iterations);
     1154                for(int i=0;i<vectors.size();++i)
     1155                {
     1156                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
     1157                }
     1158                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1159                dbvt.optimizeTopDown();
     1160                printf("[13] culling(OCL+fullsort): ");
     1161                wallclock.reset();     
     1162                for(int i=0;i<cfgBenchmark13_Iterations;++i)
     1163                {
     1164                        static const btScalar   offset=0;
     1165                        policy.m_depth=-SIMD_INFINITY;
     1166                        dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
     1167                }
     1168                const int       time=(int)wallclock.getTimeMilliseconds();
     1169                const int       t=cfgBenchmark13_Iterations;
     1170                printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
     1171        }
     1172        if(cfgBenchmark14_Enable)
    11731173        {// Benchmark 14       
    1174         srand(380843);
    1175         btDbvt                                                          dbvt;
    1176         btAlignedObjectArray<btVector3>         vectors;
    1177         btDbvtBenchmark::P14                            policy;
    1178         vectors.resize(cfgBenchmark14_Iterations);
    1179         for(int i=0;i<vectors.size();++i)
    1180                 {
    1181                 vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
    1182                 }
    1183         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1184         dbvt.optimizeTopDown();
    1185         policy.m_nodes.reserve(cfgLeaves);
    1186         printf("[14] culling(OCL+qsort): ");
    1187         wallclock.reset();     
    1188         for(int i=0;i<cfgBenchmark14_Iterations;++i)
    1189                 {
    1190                 static const btScalar   offset=0;
    1191                 policy.m_nodes.resize(0);
    1192                 dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
    1193                 policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
    1194                 }
    1195         const int       time=(int)wallclock.getTimeMilliseconds();
    1196         const int       t=cfgBenchmark14_Iterations;
    1197         printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
    1198         }
    1199 if(cfgBenchmark15_Enable)
     1174                srand(380843);
     1175                btDbvt                                                          dbvt;
     1176                btAlignedObjectArray<btVector3>         vectors;
     1177                btDbvtBenchmark::P14                            policy;
     1178                vectors.resize(cfgBenchmark14_Iterations);
     1179                for(int i=0;i<vectors.size();++i)
     1180                {
     1181                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
     1182                }
     1183                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1184                dbvt.optimizeTopDown();
     1185                policy.m_nodes.reserve(cfgLeaves);
     1186                printf("[14] culling(OCL+qsort): ");
     1187                wallclock.reset();     
     1188                for(int i=0;i<cfgBenchmark14_Iterations;++i)
     1189                {
     1190                        static const btScalar   offset=0;
     1191                        policy.m_nodes.resize(0);
     1192                        dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
     1193                        policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
     1194                }
     1195                const int       time=(int)wallclock.getTimeMilliseconds();
     1196                const int       t=cfgBenchmark14_Iterations;
     1197                printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
     1198        }
     1199        if(cfgBenchmark15_Enable)
    12001200        {// Benchmark 15       
    1201         srand(380843);
    1202         btDbvt                                                          dbvt;
    1203         btAlignedObjectArray<btVector3>         vectors;
    1204         btDbvtBenchmark::P15                            policy;
    1205         vectors.resize(cfgBenchmark15_Iterations);
    1206         for(int i=0;i<vectors.size();++i)
    1207                 {
    1208                 vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
    1209                 }
    1210         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1211         dbvt.optimizeTopDown();
    1212         policy.m_nodes.reserve(cfgLeaves);
    1213         printf("[15] culling(KDOP+qsort): ");
    1214         wallclock.reset();     
    1215         for(int i=0;i<cfgBenchmark15_Iterations;++i)
    1216                 {
    1217                 static const btScalar   offset=0;
    1218                 policy.m_nodes.resize(0);
    1219                 policy.m_axis=vectors[i];
    1220                 dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
    1221                 policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
    1222                 }
    1223         const int       time=(int)wallclock.getTimeMilliseconds();
    1224         const int       t=cfgBenchmark15_Iterations;
    1225         printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
    1226         }
    1227 if(cfgBenchmark16_Enable)
     1201                srand(380843);
     1202                btDbvt                                                          dbvt;
     1203                btAlignedObjectArray<btVector3>         vectors;
     1204                btDbvtBenchmark::P15                            policy;
     1205                vectors.resize(cfgBenchmark15_Iterations);
     1206                for(int i=0;i<vectors.size();++i)
     1207                {
     1208                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
     1209                }
     1210                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1211                dbvt.optimizeTopDown();
     1212                policy.m_nodes.reserve(cfgLeaves);
     1213                printf("[15] culling(KDOP+qsort): ");
     1214                wallclock.reset();     
     1215                for(int i=0;i<cfgBenchmark15_Iterations;++i)
     1216                {
     1217                        static const btScalar   offset=0;
     1218                        policy.m_nodes.resize(0);
     1219                        policy.m_axis=vectors[i];
     1220                        dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
     1221                        policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
     1222                }
     1223                const int       time=(int)wallclock.getTimeMilliseconds();
     1224                const int       t=cfgBenchmark15_Iterations;
     1225                printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
     1226        }
     1227        if(cfgBenchmark16_Enable)
    12281228        {// Benchmark 16       
    1229         srand(380843);
    1230         btDbvt                                                          dbvt;
    1231         btAlignedObjectArray<btDbvtNode*>       batch;
    1232         btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
    1233         dbvt.optimizeTopDown();
    1234         batch.reserve(cfgBenchmark16_BatchCount);
    1235         printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
    1236         wallclock.reset();
    1237         for(int i=0;i<cfgBenchmark16_Passes;++i)
    1238                 {
    1239                 for(int j=0;j<cfgBenchmark16_BatchCount;++j)
    1240                         {
    1241                         batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
    1242                         }
    1243                 for(int j=0;j<cfgBenchmark16_BatchCount;++j)
    1244                         {
    1245                         dbvt.remove(batch[j]);
    1246                         }
    1247                 batch.resize(0);
    1248                 }
    1249         const int       time=(int)wallclock.getTimeMilliseconds();
    1250         const int       ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
    1251         printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
    1252         }
    1253 if(cfgBenchmark17_Enable)
     1229                srand(380843);
     1230                btDbvt                                                          dbvt;
     1231                btAlignedObjectArray<btDbvtNode*>       batch;
     1232                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
     1233                dbvt.optimizeTopDown();
     1234                batch.reserve(cfgBenchmark16_BatchCount);
     1235                printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
     1236                wallclock.reset();
     1237                for(int i=0;i<cfgBenchmark16_Passes;++i)
     1238                {
     1239                        for(int j=0;j<cfgBenchmark16_BatchCount;++j)
     1240                        {
     1241                                batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
     1242                        }
     1243                        for(int j=0;j<cfgBenchmark16_BatchCount;++j)
     1244                        {
     1245                                dbvt.remove(batch[j]);
     1246                        }
     1247                        batch.resize(0);
     1248                }
     1249                const int       time=(int)wallclock.getTimeMilliseconds();
     1250                const int       ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
     1251                printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
     1252        }
     1253        if(cfgBenchmark17_Enable)
    12541254        {// Benchmark 17
    1255         srand(380843);
    1256         btAlignedObjectArray<btDbvtVolume>      volumes;
    1257         btAlignedObjectArray<int>                       results;
    1258         btAlignedObjectArray<int>                       indices;
    1259         volumes.resize(cfgLeaves);
    1260         results.resize(cfgLeaves);
    1261         indices.resize(cfgLeaves);
    1262         for(int i=0;i<cfgLeaves;++i)
    1263                 {
    1264                 indices[i]=i;
    1265                 volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
    1266                 }
    1267         for(int i=0;i<cfgLeaves;++i)
    1268                 {
    1269                 btSwap(indices[i],indices[rand()%cfgLeaves]);
    1270                 }
    1271         printf("[17] btDbvtVolume select: ");
    1272         wallclock.reset();
    1273         for(int i=0;i<cfgBenchmark17_Iterations;++i)
    1274                 {
    1275                 for(int j=0;j<cfgLeaves;++j)
    1276                         {
    1277                         for(int k=0;k<cfgLeaves;++k)
     1255                srand(380843);
     1256                btAlignedObjectArray<btDbvtVolume>      volumes;
     1257                btAlignedObjectArray<int>                       results;
     1258                btAlignedObjectArray<int>                       indices;
     1259                volumes.resize(cfgLeaves);
     1260                results.resize(cfgLeaves);
     1261                indices.resize(cfgLeaves);
     1262                for(int i=0;i<cfgLeaves;++i)
     1263                {
     1264                        indices[i]=i;
     1265                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
     1266                }
     1267                for(int i=0;i<cfgLeaves;++i)
     1268                {
     1269                        btSwap(indices[i],indices[rand()%cfgLeaves]);
     1270                }
     1271                printf("[17] btDbvtVolume select: ");
     1272                wallclock.reset();
     1273                for(int i=0;i<cfgBenchmark17_Iterations;++i)
     1274                {
     1275                        for(int j=0;j<cfgLeaves;++j)
     1276                        {
     1277                                for(int k=0;k<cfgLeaves;++k)
    12781278                                {
    1279                                 const int idx=indices[k];
    1280                                 results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
     1279                                        const int idx=indices[k];
     1280                                        results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
    12811281                                }
    12821282                        }
    12831283                }
    1284         const int time=(int)wallclock.getTimeMilliseconds();
    1285         printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
    1286         }
    1287 printf("\r\n\r\n");
     1284                const int time=(int)wallclock.getTimeMilliseconds();
     1285                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
     1286        }
     1287        printf("\r\n\r\n");
    12881288}
    12891289#endif

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h

    r2192 r2430  
    2121#include "LinearMath/btVector3.h"
    2222#include "LinearMath/btTransform.h"
     23#include "LinearMath/btAabbUtil2.h"
    2324
    2425//
    … …  
    3334// Template implementation of ICollide
    3435#ifdef WIN32
    35         #if (defined (_MSC_VER) && _MSC_VER >= 1400)
    36         #define DBVT_USE_TEMPLATE               1
    37         #else
    38         #define DBVT_USE_TEMPLATE               0
    39 #endif
     36#if (defined (_MSC_VER) && _MSC_VER >= 1400)
     37#define DBVT_USE_TEMPLATE               1
    4038#else
    4139#define DBVT_USE_TEMPLATE               0
    4240#endif
     41#else
     42#define DBVT_USE_TEMPLATE               0
     43#endif
    4344
    4445// Use only intrinsics instead of inline asm
    … …  
    5354// Inlining
    5455#define DBVT_INLINE                             SIMD_FORCE_INLINE
    55 // Align
    56 #ifdef WIN32
    57 #define DBVT_ALIGN                              __declspec(align(16))
    58 #else
    59 #define DBVT_ALIGN
    60 #endif
    6156
    6257// Specific methods implementation
    … …  
    135130struct  btDbvtAabbMm
    136131{
    137 DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
    138 DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
    139 DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
    140 DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
    141 DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
    142 static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
    143 static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
    144 static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
    145 static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
    146 static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
    147 DBVT_INLINE void                                Expand(const btVector3& e);
    148 DBVT_INLINE void                                SignedExpand(const btVector3& e);
    149 DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
    150 DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
    151 DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
    152 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    153                                                                                         const btDbvtAabbMm& b);
    154 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    155                                                                                         const btDbvtAabbMm& b,
    156                                                                                         const btTransform& xform);
    157 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    158                                                                                         const btVector3& b);
    159 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    160                                                                                         const btVector3& org,
    161                                                                                         const btVector3& invdir,
    162                                                                                         const unsigned* signs);
    163 DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
    164                                                                                         const btDbvtAabbMm& b);
    165 DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
    166                                                                                         const btDbvtAabbMm& a,
    167                                                                                         const btDbvtAabbMm& b);
    168 DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
    169                                                                                         const btDbvtAabbMm& b,
    170                                                                                         btDbvtAabbMm& r);
    171 DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
    172                                                                                         const btDbvtAabbMm& b);
     132        DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
     133        DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
     134        DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
     135        DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
     136        DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
     137        static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
     138        static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
     139        static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
     140        static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
     141        static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
     142        DBVT_INLINE void                                Expand(const btVector3& e);
     143        DBVT_INLINE void                                SignedExpand(const btVector3& e);
     144        DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
     145        DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
     146        DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
     147        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     148                const btDbvtAabbMm& b);
     149        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     150                const btDbvtAabbMm& b,
     151                const btTransform& xform);
     152        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     153                const btVector3& b);
     154
     155        DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
     156                const btDbvtAabbMm& b);
     157        DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
     158                const btDbvtAabbMm& a,
     159                const btDbvtAabbMm& b);
     160        DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
     161                const btDbvtAabbMm& b,
     162                btDbvtAabbMm& r);
     163        DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
     164                const btDbvtAabbMm& b);
    173165private:
    174 DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
     166        DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
    175167private:
    176 btVector3       mi,mx;
     168        btVector3       mi,mx;
    177169};
    178170
    … …  
    187179        DBVT_INLINE bool        isleaf() const          { return(childs[1]==0); }
    188180        DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
    189         union   {
    190                         btDbvtNode*     childs[2];
    191                         void*   data;
    192                         int             dataAsInt;
    193                         };
     181        union
     182        {
     183                btDbvtNode*     childs[2];
     184                void*   data;
     185                int             dataAsInt;
     186        };
    194187};
    195188
    … …  
    198191///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
    199192struct  btDbvt
    200         {
     193{
    201194        /* Stack element        */
    202195        struct  sStkNN
    203                 {
     196        {
    204197                const btDbvtNode*       a;
    205198                const btDbvtNode*       b;
    206199                sStkNN() {}
    207200                sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
    208                 };
     201        };
    209202        struct  sStkNP
    210                 {
     203        {
    211204                const btDbvtNode*       node;
    212205                int                     mask;
    213206                sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
    214                 };
     207        };
    215208        struct  sStkNPS
    216                 {
     209        {
    217210                const btDbvtNode*       node;
    218211                int                     mask;
    … …  
    220213                sStkNPS() {}
    221214                sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
    222                 };
     215        };
    223216        struct  sStkCLN
    224                 {
     217        {
    225218                const btDbvtNode*       node;
    226219                btDbvtNode*             parent;
    227220                sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
    228                 };
     221        };
    229222        // Policies/Interfaces
    230                        
     223
    231224        /* ICollide     */
    232225        struct  ICollide
    233                 {               
     226        {               
    234227                DBVT_VIRTUAL_DTOR(ICollide)
    235                 DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
     228                        DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
    236229                DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
    237230                DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
    238231                DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
    239232                DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
    240                 };
     233        };
    241234        /* IWriter      */
    242235        struct  IWriter
    243                 {
     236        {
    244237                virtual ~IWriter() {}
    245238                virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
    246239                virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
    247240                virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
    248                 };
     241        };
    249242        /* IClone       */
    250243        struct  IClone
    251                 {
     244        {
    252245                virtual ~IClone()       {}
    253246                virtual void            CloneLeaf(btDbvtNode*) {}
    254                 };
    255                
     247        };
     248
    256249        // Constants
    257250        enum    {
    258                         SIMPLE_STACKSIZE        =       64,
    259                         DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
    260                         };
    261                
     251                SIMPLE_STACKSIZE        =       64,
     252                DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
     253        };
     254
    262255        // Fields
    263256        btDbvtNode*             m_root;
    … …  
    266259        int                             m_leaves;
    267260        unsigned                m_opath;
     261
     262       
     263        btAlignedObjectArray<sStkNN>    m_stkStack;
     264
     265
    268266        // Methods
    269                                         btDbvt();
    270                                         ~btDbvt();
     267        btDbvt();
     268        ~btDbvt();
    271269        void                    clear();
    272270        bool                    empty() const { return(0==m_root); }
    … …  
    276274        btDbvtNode*             insert(const btDbvtVolume& box,void* data);
    277275        void                    update(btDbvtNode* leaf,int lookahead=-1);
    278         void                    update(btDbvtNode* leaf,const btDbvtVolume& volume);
    279         bool                    update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin);
    280         bool                    update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity);
    281         bool                    update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin);   
     276        void                    update(btDbvtNode* leaf,btDbvtVolume& volume);
     277        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
     278        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
     279        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); 
    282280        void                    remove(btDbvtNode* leaf);
    283281        void                    write(IWriter* iwriter) const;
    … …  
    286284        static int              countLeaves(const btDbvtNode* node);
    287285        static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
    288         #if DBVT_ENABLE_BENCHMARK
     286#if DBVT_ENABLE_BENCHMARK
    289287        static void             benchmark();
    290         #else
     288#else
    291289        static void             benchmark(){}
    292         #endif
     290#endif
    293291        // DBVT_IPOLICY must support ICollide policy/interface
    294292        DBVT_PREFIX
    295         static void             enumNodes(      const btDbvtNode* root,
    296                                                                 DBVT_IPOLICY);
     293                static void             enumNodes(      const btDbvtNode* root,
     294                DBVT_IPOLICY);
    297295        DBVT_PREFIX
    298         static void             enumLeaves(     const btDbvtNode* root,
    299                                                                 DBVT_IPOLICY);
     296                static void             enumLeaves(     const btDbvtNode* root,
     297                DBVT_IPOLICY);
    300298        DBVT_PREFIX
    301         static void             collideTT(      const btDbvtNode* root0,
    302                                                                 const btDbvtNode* root1,
    303                                                                 DBVT_IPOLICY);
     299                void            collideTT(      const btDbvtNode* root0,
     300                const btDbvtNode* root1,
     301                DBVT_IPOLICY);
     302
    304303        DBVT_PREFIX
    305         static void             collideTT(      const btDbvtNode* root0,
    306                                                                 const btDbvtNode* root1,
    307                                                                 const btTransform& xform,
    308                                                                 DBVT_IPOLICY);
     304                void            collideTTpersistentStack(       const btDbvtNode* root0,
     305                  const btDbvtNode* root1,
     306                  DBVT_IPOLICY);
     307
    309308        DBVT_PREFIX
    310         static void             collideTT(      const btDbvtNode* root0,
    311                                                                 const btTransform& xform0,
    312                                                                 const btDbvtNode* root1,
    313                                                                 const btTransform& xform1,
    314                                                                 DBVT_IPOLICY);
     309                void            collideTT(      const btDbvtNode* root0,
     310                const btDbvtNode* root1,
     311                const btTransform& xform,
     312                DBVT_IPOLICY);
    315313        DBVT_PREFIX
    316         static void             collideTV(      const btDbvtNode* root,
    317                                                                 const btDbvtVolume& volume,
    318                                                                 DBVT_IPOLICY);
     314                void            collideTT(      const btDbvtNode* root0,
     315                const btTransform& xform0,
     316                const btDbvtNode* root1,
     317                const btTransform& xform1,
     318                DBVT_IPOLICY);
    319319        DBVT_PREFIX
    320         static void             collideRAY(     const btDbvtNode* root,
    321                                                                 const btVector3& origin,
    322                                                                 const btVector3& direction,
    323                                                                 DBVT_IPOLICY);
     320                void            collideTV(      const btDbvtNode* root,
     321                const btDbvtVolume& volume,
     322                DBVT_IPOLICY);
     323        ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
     324        ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
    324325        DBVT_PREFIX
    325         static void             collideKDOP(const btDbvtNode* root,
    326                                                                 const btVector3* normals,
    327                                                                 const btScalar* offsets,
    328                                                                 int count,
    329                                                                 DBVT_IPOLICY);
     326                static void             rayTest(        const btDbvtNode* root,
     327                const btVector3& rayFrom,
     328                const btVector3& rayTo,
     329                DBVT_IPOLICY);
     330        ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
     331        ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
    330332        DBVT_PREFIX
    331         static void             collideOCL(     const btDbvtNode* root,
    332                                                                 const btVector3* normals,
    333                                                                 const btScalar* offsets,
    334                                                                 const btVector3& sortaxis,
    335                                                                 int count,                                                             
    336                                                                 DBVT_IPOLICY,
    337                                                                 bool fullsort=true);
     333                void            rayTestInternal(        const btDbvtNode* root,
     334                                                                const btVector3& rayFrom,
     335                                                                const btVector3& rayTo,
     336                                                                const btVector3& rayDirectionInverse,
     337                                                                unsigned int signs[3],
     338                                                                btScalar lambda_max,
     339                                                                const btVector3& aabbMin,
     340                                                                const btVector3& aabbMax,
     341                                                                DBVT_IPOLICY) const;
     342
    338343        DBVT_PREFIX
    339         static void             collideTU(      const btDbvtNode* root,
    340                                                                 DBVT_IPOLICY);
     344                static void             collideKDOP(const btDbvtNode* root,
     345                const btVector3* normals,
     346                const btScalar* offsets,
     347                int count,
     348                DBVT_IPOLICY);
     349        DBVT_PREFIX
     350                static void             collideOCL(     const btDbvtNode* root,
     351                const btVector3* normals,
     352                const btScalar* offsets,
     353                const btVector3& sortaxis,
     354                int count,                                                             
     355                DBVT_IPOLICY,
     356                bool fullsort=true);
     357        DBVT_PREFIX
     358                static void             collideTU(      const btDbvtNode* root,
     359                DBVT_IPOLICY);
    341360        // Helpers     
    342361        static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
    343                 {
     362        {
    344363                int     m=0;
    345364                while(l<h)
    346                         {
     365                {
    347366                        m=(l+h)>>1;
    348367                        if(a[i[m]].value>=v) l=m+1; else h=m;
    349                         }
     368                }
    350369                return(h);
    351                 }
     370        }
    352371        static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
    353                                                                                 btAlignedObjectArray<sStkNPS>& stock,
    354                                                                                 const sStkNPS& value)
    355                 {
     372                btAlignedObjectArray<sStkNPS>& stock,
     373                const sStkNPS& value)
     374        {
    356375                int     i;
    357376                if(ifree.size()>0)
    358                         { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
    359                         else
    360                         { i=stock.size();stock.push_back(value); }
     377                { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
     378                else
     379                { i=stock.size();stock.push_back(value); }
    361380                return(i);
    362                 }
     381        }
    363382        //
    364         private:
    365                                         btDbvt(const btDbvt&)   {}     
    366         };
     383private:
     384        btDbvt(const btDbvt&)   {}     
     385};
    367386
    368387//
    … …  
    373392inline btDbvtAabbMm                     btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
    374393{
    375 btDbvtAabbMm box;
    376 box.mi=c-e;box.mx=c+e;
    377 return(box);
    378 }
    379        
     394        btDbvtAabbMm box;
     395        box.mi=c-e;box.mx=c+e;
     396        return(box);
     397}
     398
    380399//
    381400inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
    382401{
    383 return(FromCE(c,btVector3(r,r,r)));
    384 }
    385        
     402        return(FromCE(c,btVector3(r,r,r)));
     403}
     404
    386405//
    387406inline btDbvtAabbMm                     btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
    388407{
    389 btDbvtAabbMm box;
    390 box.mi=mi;box.mx=mx;
    391 return(box);
    392 }
    393        
     408        btDbvtAabbMm box;
     409        box.mi=mi;box.mx=mx;
     410        return(box);
     411}
     412
    394413//
    395414inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
    396415{
    397 btDbvtAabbMm box;
    398 box.mi=box.mx=pts[0];
    399 for(int i=1;i<n;++i)
    400         {
    401         box.mi.setMin(pts[i]);
    402         box.mx.setMax(pts[i]);
     416        btDbvtAabbMm box;
     417        box.mi=box.mx=pts[0];
     418        for(int i=1;i<n;++i)
     419        {
     420                box.mi.setMin(pts[i]);
     421                box.mx.setMax(pts[i]);
    403422        }
    404 return(box);
     423        return(box);
    405424}
    406425
    … …  
    408427inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
    409428{
    410 btDbvtAabbMm box;
    411 box.mi=box.mx=*ppts[0];
    412 for(int i=1;i<n;++i)
    413         {
    414         box.mi.setMin(*ppts[i]);
    415         box.mx.setMax(*ppts[i]);
     429        btDbvtAabbMm box;
     430        box.mi=box.mx=*ppts[0];
     431        for(int i=1;i<n;++i)
     432        {
     433                box.mi.setMin(*ppts[i]);
     434                box.mx.setMax(*ppts[i]);
    416435        }
    417 return(box);
     436        return(box);
    418437}
    419438
    … …  
    421440DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
    422441{
    423 mi-=e;mx+=e;
    424 }
    425        
     442        mi-=e;mx+=e;
     443}
     444
    426445//
    427446DBVT_INLINE void                btDbvtAabbMm::SignedExpand(const btVector3& e)
    428447{
    429 if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
    430 if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
    431 if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
    432 }
    433        
     448        if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
     449        if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
     450        if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
     451}
     452
    434453//
    435454DBVT_INLINE bool                btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
    436455{
    437 return( (mi.x()<=a.mi.x())&&
     456        return( (mi.x()<=a.mi.x())&&
    438457                (mi.y()<=a.mi.y())&&
    439458                (mi.z()<=a.mi.z())&&
    … …  
    446465DBVT_INLINE int         btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
    447466{
    448 btVector3                       pi,px;
    449 switch(s)
     467        btVector3                       pi,px;
     468        switch(s)
    450469        {
    451470        case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
    452                                                 pi=btVector3(mx.x(),mx.y(),mx.z());break;
     471                pi=btVector3(mx.x(),mx.y(),mx.z());break;
    453472        case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
    454                                                 pi=btVector3(mi.x(),mx.y(),mx.z());break;
     473                pi=btVector3(mi.x(),mx.y(),mx.z());break;
    455474        case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
    456                                                 pi=btVector3(mx.x(),mi.y(),mx.z());break;
     475                pi=btVector3(mx.x(),mi.y(),mx.z());break;
    457476        case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
    458                                                 pi=btVector3(mi.x(),mi.y(),mx.z());break;
     477                pi=btVector3(mi.x(),mi.y(),mx.z());break;
    459478        case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
    460                                                 pi=btVector3(mx.x(),mx.y(),mi.z());break;
     479                pi=btVector3(mx.x(),mx.y(),mi.z());break;
    461480        case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
    462                                                 pi=btVector3(mi.x(),mx.y(),mi.z());break;
     481                pi=btVector3(mi.x(),mx.y(),mi.z());break;
    463482        case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
    464                                                 pi=btVector3(mx.x(),mi.y(),mi.z());break;
     483                pi=btVector3(mx.x(),mi.y(),mi.z());break;
    465484        case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
    466                                                 pi=btVector3(mi.x(),mi.y(),mi.z());break;
     485                pi=btVector3(mi.x(),mi.y(),mi.z());break;
    467486        }
    468 if((dot(n,px)+o)<0)             return(-1);
    469 if((dot(n,pi)+o)>=0)    return(+1);
    470 return(0);
     487        if((dot(n,px)+o)<0)             return(-1);
     488        if((dot(n,pi)+o)>=0)    return(+1);
     489        return(0);
    471490}
    472491
    … …  
    474493DBVT_INLINE btScalar    btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
    475494{
    476 const btVector3*        b[]={&mx,&mi};
    477 const btVector3         p(      b[(signs>>0)&1]->x(),
    478                                                 b[(signs>>1)&1]->y(),
    479                                                 b[(signs>>2)&1]->z());
    480 return(dot(p,v));
     495        const btVector3*        b[]={&mx,&mi};
     496        const btVector3         p(      b[(signs>>0)&1]->x(),
     497                b[(signs>>1)&1]->y(),
     498                b[(signs>>2)&1]->z());
     499        return(dot(p,v));
    481500}
    482501
    … …  
    484503DBVT_INLINE void                btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
    485504{
    486 for(int i=0;i<3;++i)
    487         {
    488         if(d[i]<0)
     505        for(int i=0;i<3;++i)
     506        {
     507                if(d[i]<0)
    489508                { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
    490509                else
    … …  
    492511        }
    493512}
    494        
     513
    495514//
    496515DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    497                                                                         const btDbvtAabbMm& b)
     516                                                                  const btDbvtAabbMm& b)
    498517{
    499518#if     DBVT_INT0_IMPL == DBVT_IMPL_SSE
    500 const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
    501                                                                 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
    502 const __int32*  pu((const __int32*)&rt);
    503 return((pu[0]|pu[1]|pu[2])==0);
     519        const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
     520                _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
     521        const __int32*  pu((const __int32*)&rt);
     522        return((pu[0]|pu[1]|pu[2])==0);
    504523#else
    505 return( (a.mi.x()<=b.mx.x())&&
     524        return( (a.mi.x()<=b.mx.x())&&
    506525                (a.mx.x()>=b.mi.x())&&
    507526                (a.mi.y()<=b.mx.y())&&
    … …  
    514533//
    515534DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    516                                                                         const btDbvtAabbMm& b,
    517                                                                         const btTransform& xform)
    518 {
    519 const btVector3         d0=xform*b.Center()-a.Center();
    520 const btVector3         d1=d0*xform.getBasis();
    521 btScalar                        s0[2]={0,0};
    522 btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
    523 a.AddSpan(d0,s0[0],s0[1]);
    524 b.AddSpan(d1,s1[0],s1[1]);
    525 if(s0[0]>(s1[1])) return(false);
    526 if(s0[1]<(s1[0])) return(false);
    527 return(true);
     535                                                                  const btDbvtAabbMm& b,
     536                                                                  const btTransform& xform)
     537{
     538        const btVector3         d0=xform*b.Center()-a.Center();
     539        const btVector3         d1=d0*xform.getBasis();
     540        btScalar                        s0[2]={0,0};
     541        btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
     542        a.AddSpan(d0,s0[0],s0[1]);
     543        b.AddSpan(d1,s1[0],s1[1]);
     544        if(s0[0]>(s1[1])) return(false);
     545        if(s0[1]<(s1[0])) return(false);
     546        return(true);
    528547}
    529548
    530549//
    531550DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    532                                                                         const btVector3& b)
    533 {
    534 return( (b.x()>=a.mi.x())&&
     551                                                                  const btVector3& b)
     552{
     553        return( (b.x()>=a.mi.x())&&
    535554                (b.y()>=a.mi.y())&&
    536555                (b.z()>=a.mi.z())&&
    … …  
    540559}
    541560
    542 //
    543 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    544                                                                         const btVector3& org,
    545                                                                         const btVector3& invdir,
    546                                                                         const unsigned* signs)
    547 {
    548 #if 0
    549 const btVector3         b0((a.mi-org)*invdir);
    550 const btVector3         b1((a.mx-org)*invdir);
    551 const btVector3         tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2]));
    552 const btVector3         tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2]));
    553 const btScalar          tin=btMax(tmin[0],btMax(tmin[1],tmin[2]));
    554 const btScalar          tout=btMin(tmax[0],btMin(tmax[1],tmax[2]));
    555 return(tin<tout);
    556 #else
    557 const btVector3*        bounds[2]={&a.mi,&a.mx};
    558 btScalar                        txmin=(bounds[  signs[0]]->x()-org[0])*invdir[0];
    559 btScalar                        txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
    560 const btScalar          tymin=(bounds[  signs[1]]->y()-org[1])*invdir[1];
    561 const btScalar          tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
    562 if((txmin>tymax)||(tymin>txmax)) return(false);
    563 if(tymin>txmin) txmin=tymin;
    564 if(tymax<txmax) txmax=tymax;
    565 const btScalar          tzmin=(bounds[  signs[2]]->z()-org[2])*invdir[2];
    566 const btScalar          tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
    567 if((txmin>tzmax)||(tzmin>txmax)) return(false);
    568 if(tzmin>txmin) txmin=tzmin;
    569 if(tzmax<txmax) txmax=tzmax;
    570 return(txmax>0);
    571 #endif
    572 }
    573        
     561
     562
     563
     564
     565//////////////////////////////////////
     566
     567
    574568//
    575569DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
    576                                                                         const btDbvtAabbMm& b)
    577 {
    578 const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
    579 return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
    580 }
     570                                                                  const btDbvtAabbMm& b)
     571{
     572        const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
     573        return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
     574}
     575
     576
    581577
    582578//
    583579DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
    584                                                                 const btDbvtAabbMm& a,
    585                                                                 const btDbvtAabbMm& b)
     580                                                           const btDbvtAabbMm& a,
     581                                                           const btDbvtAabbMm& b)
    586582{
    587583#if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
    588 static DBVT_ALIGN const unsigned __int32        mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
    589         // TODO: the intrinsic version is 11% slower
    590         #if DBVT_USE_INTRINSIC_SSE
     584        static ATTRIBUTE_ALIGNED16(const unsigned __int32)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
     585        ///@todo: the intrinsic version is 11% slower
     586#if DBVT_USE_INTRINSIC_SSE
     587
     588        union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
     589        {
     590           __m128               ssereg;
     591           float                floats[4];
     592           int                  ints[4];
     593        };
     594
    591595        __m128  omi(_mm_load_ps(o.mi));
    592596        omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
    … …  
    602606        ami=_mm_add_ps(ami,t0);
    603607        ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
    604         __m128  t1(_mm_movehl_ps(bmi,bmi));
     608        __m128 t1(_mm_movehl_ps(bmi,bmi));
    605609        bmi=_mm_add_ps(bmi,t1);
    606610        bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
    607         return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1);
    608         #else
    609         DBVT_ALIGN __int32      r[1];
     611       
     612        btSSEUnion tmp;
     613        tmp.ssereg = _mm_cmple_ss(bmi,ami);
     614        return tmp.ints[0]&1;
     615
     616#else
     617        ATTRIBUTE_ALIGNED16(__int32     r[1]);
    610618        __asm
    611                 {
     619        {
    612620                mov             eax,o
    613                 mov             ecx,a
    614                 mov             edx,b
    615                 movaps  xmm0,[eax]
     621                        mov             ecx,a
     622                        mov             edx,b
     623                        movaps  xmm0,[eax]
    616624                movaps  xmm5,mask
    617                 addps   xmm0,[eax+16]   
     625                        addps   xmm0,[eax+16]   
    618626                movaps  xmm1,[ecx]
    619627                movaps  xmm2,[edx]
    … …  
    621629                addps   xmm2,[edx+16]
    622630                subps   xmm1,xmm0
    623                 subps   xmm2,xmm0
    624                 andps   xmm1,xmm5
    625                 andps   xmm2,xmm5
    626                 movhlps xmm3,xmm1
    627                 movhlps xmm4,xmm2
    628                 addps   xmm1,xmm3
    629                 addps   xmm2,xmm4
    630                 pshufd  xmm3,xmm1,1
    631                 pshufd  xmm4,xmm2,1
    632                 addss   xmm1,xmm3
    633                 addss   xmm2,xmm4
    634                 cmpless xmm2,xmm1
    635                 movss   r,xmm2
    636                 }
     631                        subps   xmm2,xmm0
     632                        andps   xmm1,xmm5
     633                        andps   xmm2,xmm5
     634                        movhlps xmm3,xmm1
     635                        movhlps xmm4,xmm2
     636                        addps   xmm1,xmm3
     637                        addps   xmm2,xmm4
     638                        pshufd  xmm3,xmm1,1
     639                        pshufd  xmm4,xmm2,1
     640                        addss   xmm1,xmm3
     641                        addss   xmm2,xmm4
     642                        cmpless xmm2,xmm1
     643                        movss   r,xmm2
     644        }
    637645        return(r[0]&1);
    638         #endif
     646#endif
    639647#else
    640 return(Proximity(o,a)<Proximity(o,b)?0:1);
     648        return(Proximity(o,a)<Proximity(o,b)?0:1);
    641649#endif
    642650}
    … …  
    644652//
    645653DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
    646                                                                 const btDbvtAabbMm& b,
    647                                                                 btDbvtAabbMm& r)
     654                                                          const btDbvtAabbMm& b,
     655                                                          btDbvtAabbMm& r)
    648656{
    649657#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
    650 __m128  ami(_mm_load_ps(a.mi));
    651 __m128  amx(_mm_load_ps(a.mx));
    652 __m128  bmi(_mm_load_ps(b.mi));
    653 __m128  bmx(_mm_load_ps(b.mx));
    654 ami=_mm_min_ps(ami,bmi);
    655 amx=_mm_max_ps(amx,bmx);
    656 _mm_store_ps(r.mi,ami);
    657 _mm_store_ps(r.mx,amx);
     658        __m128  ami(_mm_load_ps(a.mi));
     659        __m128  amx(_mm_load_ps(a.mx));
     660        __m128  bmi(_mm_load_ps(b.mi));
     661        __m128  bmx(_mm_load_ps(b.mx));
     662        ami=_mm_min_ps(ami,bmi);
     663        amx=_mm_max_ps(amx,bmx);
     664        _mm_store_ps(r.mi,ami);
     665        _mm_store_ps(r.mx,amx);
    658666#else
    659 for(int i=0;i<3;++i)
    660         {
    661         if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
    662         if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
     667        for(int i=0;i<3;++i)
     668        {
     669                if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
     670                if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
    663671        }
    664672#endif
    … …  
    667675//
    668676DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
    669                                                                         const btDbvtAabbMm& b)
    670 {
    671 return( (a.mi.x()!=b.mi.x())||
     677                                                                 const btDbvtAabbMm& b)
     678{
     679        return( (a.mi.x()!=b.mi.x())||
    672680                (a.mi.y()!=b.mi.y())||
    673681                (a.mi.z()!=b.mi.z())||
    … …  
    684692DBVT_PREFIX
    685693inline void             btDbvt::enumNodes(      const btDbvtNode* root,
    686                                                                         DBVT_IPOLICY)
    687 {
    688 DBVT_CHECKTYPE
    689 policy.Process(root);
    690 if(root->isinternal())
    691         {
    692         enumNodes(root->childs[0],policy);
    693         enumNodes(root->childs[1],policy);
     694                                                                  DBVT_IPOLICY)
     695{
     696        DBVT_CHECKTYPE
     697                policy.Process(root);
     698        if(root->isinternal())
     699        {
     700                enumNodes(root->childs[0],policy);
     701                enumNodes(root->childs[1],policy);
    694702        }
    695703}
    … …  
    698706DBVT_PREFIX
    699707inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
    700                                                                         DBVT_IPOLICY)
    701 {
    702 DBVT_CHECKTYPE
    703 if(root->isinternal())
    704         {
    705         enumLeaves(root->childs[0],policy);
    706         enumLeaves(root->childs[1],policy);
    707         }
    708         else
    709         {
    710         policy.Process(root);
    711         }
     708                                                                   DBVT_IPOLICY)
     709{
     710        DBVT_CHECKTYPE
     711                if(root->isinternal())
     712                {
     713                        enumLeaves(root->childs[0],policy);
     714                        enumLeaves(root->childs[1],policy);
     715                }
     716                else
     717                {
     718                        policy.Process(root);
     719                }
    712720}
    713721
    … …  
    715723DBVT_PREFIX
    716724inline void             btDbvt::collideTT(      const btDbvtNode* root0,
    717                                                                         const btDbvtNode* root1,
    718                                                                         DBVT_IPOLICY)
    719 {
    720 DBVT_CHECKTYPE
    721 if(root0&&root1)
    722         {
    723         btAlignedObjectArray<sStkNN>    stack;
    724         int                                                             depth=1;
    725         int                                                             treshold=DOUBLE_STACKSIZE-4;
    726         stack.resize(DOUBLE_STACKSIZE);
    727         stack[0]=sStkNN(root0,root1);
    728         do      {               
    729                 sStkNN  p=stack[--depth];
    730                 if(depth>treshold)
     725                                                                  const btDbvtNode* root1,
     726                                                                  DBVT_IPOLICY)
     727{
     728        DBVT_CHECKTYPE
     729                if(root0&&root1)
     730                {
     731                        int                                                             depth=1;
     732                        int                                                             treshold=DOUBLE_STACKSIZE-4;
     733                        btAlignedObjectArray<sStkNN>    stkStack;
     734                        stkStack.resize(DOUBLE_STACKSIZE);
     735                        stkStack[0]=sStkNN(root0,root1);
     736                        do      {               
     737                                sStkNN  p=stkStack[--depth];
     738                                if(depth>treshold)
     739                                {
     740                                        stkStack.resize(stkStack.size()*2);
     741                                        treshold=stkStack.size()-4;
     742                                }
     743                                if(p.a==p.b)
     744                                {
     745                                        if(p.a->isinternal())
     746                                        {
     747                                                stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
     748                                                stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
     749                                                stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
     750                                        }
     751                                }
     752                                else if(Intersect(p.a->volume,p.b->volume))
     753                                {
     754                                        if(p.a->isinternal())
     755                                        {
     756                                                if(p.b->isinternal())
     757                                                {
     758                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
     759                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
     760                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
     761                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
     762                                                }
     763                                                else
     764                                                {
     765                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
     766                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
     767                                                }
     768                                        }
     769                                        else
     770                                        {
     771                                                if(p.b->isinternal())
     772                                                {
     773                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
     774                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
     775                                                }
     776                                                else
     777                                                {
     778                                                        policy.Process(p.a,p.b);
     779                                                }
     780                                        }
     781                                }
     782                        } while(depth);
     783                }
     784}
     785
     786
     787
     788DBVT_PREFIX
     789inline void             btDbvt::collideTTpersistentStack(       const btDbvtNode* root0,
     790                                                                  const btDbvtNode* root1,
     791                                                                  DBVT_IPOLICY)
     792{
     793        DBVT_CHECKTYPE
     794                if(root0&&root1)
     795                {
     796                        int                                                             depth=1;
     797                        int                                                             treshold=DOUBLE_STACKSIZE-4;
     798                       
     799                        m_stkStack.resize(DOUBLE_STACKSIZE);
     800                        m_stkStack[0]=sStkNN(root0,root1);
     801                        do      {               
     802                                sStkNN  p=m_stkStack[--depth];
     803                                if(depth>treshold)
     804                                {
     805                                        m_stkStack.resize(m_stkStack.size()*2);
     806                                        treshold=m_stkStack.size()-4;
     807                                }
     808                                if(p.a==p.b)
     809                                {
     810                                        if(p.a->isinternal())
     811                                        {
     812                                                m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
     813                                                m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
     814                                                m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
     815                                        }
     816                                }
     817                                else if(Intersect(p.a->volume,p.b->volume))
     818                                {
     819                                        if(p.a->isinternal())
     820                                        {
     821                                                if(p.b->isinternal())
     822                                                {
     823                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
     824                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
     825                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
     826                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
     827                                                }
     828                                                else
     829                                                {
     830                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
     831                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
     832                                                }
     833                                        }
     834                                        else
     835                                        {
     836                                                if(p.b->isinternal())
     837                                                {
     838                                                        m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
     839                                                        m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
     840                                                }
     841                                                else
     842                                                {
     843                                                        policy.Process(p.a,p.b);
     844                                                }
     845                                        }
     846                                }
     847                        } while(depth);
     848                }
     849}
     850
     851
     852//
     853DBVT_PREFIX
     854inline void             btDbvt::collideTT(      const btDbvtNode* root0,
     855                                                                  const btDbvtNode* root1,
     856                                                                  const btTransform& xform,
     857                                                                  DBVT_IPOLICY)
     858{
     859        DBVT_CHECKTYPE
     860                if(root0&&root1)
     861                {
     862                        int                                                             depth=1;
     863                        int                                                             treshold=DOUBLE_STACKSIZE-4;
     864                        btAlignedObjectArray<sStkNN>    stkStack;
     865                        stkStack.resize(DOUBLE_STACKSIZE);
     866                        stkStack[0]=sStkNN(root0,root1);
     867                        do      {
     868                                sStkNN  p=stkStack[--depth];
     869                                if(Intersect(p.a->volume,p.b->volume,xform))
     870                                {
     871                                        if(depth>treshold)
     872                                        {
     873                                                stkStack.resize(stkStack.size()*2);
     874                                                treshold=stkStack.size()-4;
     875                                        }
     876                                        if(p.a->isinternal())
     877                                        {
     878                                                if(p.b->isinternal())
     879                                                {                                       
     880                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
     881                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
     882                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
     883                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
     884                                                }
     885                                                else
     886                                                {
     887                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
     888                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
     889                                                }
     890                                        }
     891                                        else
     892                                        {
     893                                                if(p.b->isinternal())
     894                                                {
     895                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
     896                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
     897                                                }
     898                                                else
     899                                                {
     900                                                        policy.Process(p.a,p.b);
     901                                                }
     902                                        }
     903                                }
     904                        } while(depth);
     905                }
     906}
     907
     908//
     909DBVT_PREFIX
     910inline void             btDbvt::collideTT(      const btDbvtNode* root0,
     911                                                                  const btTransform& xform0,
     912                                                                  const btDbvtNode* root1,
     913                                                                  const btTransform& xform1,
     914                                                                  DBVT_IPOLICY)
     915{
     916        const btTransform       xform=xform0.inverse()*xform1;
     917        collideTT(root0,root1,xform,policy);
     918}
     919
     920//
     921DBVT_PREFIX
     922inline void             btDbvt::collideTV(      const btDbvtNode* root,
     923                                                                  const btDbvtVolume& vol,
     924                                                                  DBVT_IPOLICY)
     925{
     926        DBVT_CHECKTYPE
     927                if(root)
     928                {
     929                        ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
     930                        btAlignedObjectArray<const btDbvtNode*> stack;
     931                        stack.resize(0);
     932                        stack.reserve(SIMPLE_STACKSIZE);
     933                        stack.push_back(root);
     934                        do      {
     935                                const btDbvtNode*       n=stack[stack.size()-1];
     936                                stack.pop_back();
     937                                if(Intersect(n->volume,volume))
     938                                {
     939                                        if(n->isinternal())
     940                                        {
     941                                                stack.push_back(n->childs[0]);
     942                                                stack.push_back(n->childs[1]);
     943                                        }
     944                                        else
     945                                        {
     946                                                policy.Process(n);
     947                                        }
     948                                }
     949                        } while(stack.size()>0);
     950                }
     951}
     952
     953DBVT_PREFIX
     954inline void             btDbvt::rayTestInternal(        const btDbvtNode* root,
     955                                                                const btVector3& rayFrom,
     956                                                                const btVector3& rayTo,
     957                                                                const btVector3& rayDirectionInverse,
     958                                                                unsigned int signs[3],
     959                                                                btScalar lambda_max,
     960                                                                const btVector3& aabbMin,
     961                                                                const btVector3& aabbMax,
     962                                                                DBVT_IPOLICY) const
     963{
     964        DBVT_CHECKTYPE
     965        if(root)
     966        {
     967                btVector3 resultNormal;
     968
     969                int                                                             depth=1;
     970                int                                                             treshold=DOUBLE_STACKSIZE-2;
     971                btAlignedObjectArray<const btDbvtNode*> stack;
     972                stack.resize(DOUBLE_STACKSIZE);
     973                stack[0]=root;
     974                btVector3 bounds[2];
     975                do     
     976                {
     977                        const btDbvtNode*       node=stack[--depth];
     978                        bounds[0] = node->volume.Mins()+aabbMin;
     979                        bounds[1] = node->volume.Maxs()+aabbMax;
     980                        btScalar tmin=1.f,lambda_min=0.f;
     981                        unsigned int result1=false;
     982                        result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
     983                        if(result1)
    731984                        {
    732                         stack.resize(stack.size()*2);
    733                         treshold=stack.size()-4;
    734                         }
    735                 if(p.a==p.b)
    736                         {
    737                         if(p.a->isinternal())
    738                                 {
    739                                 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
    740                                 stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
    741                                 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
    742                                 }
    743                         }
    744                 else if(Intersect(p.a->volume,p.b->volume))
    745                         {
    746                         if(p.a->isinternal())
    747                                 {
    748                                 if(p.b->isinternal())
    749                                         {
    750                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
    751                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
    752                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
    753                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
    754                                         }
    755                                         else
    756                                         {
    757                                         stack[depth++]=sStkNN(p.a->childs[0],p.b);
    758                                         stack[depth++]=sStkNN(p.a->childs[1],p.b);
    759                                         }
     985                                if(node->isinternal())
     986                                {
     987                                        if(depth>treshold)
     988                                        {
     989                                                stack.resize(stack.size()*2);
     990                                                treshold=stack.size()-2;
     991                                        }
     992                                        stack[depth++]=node->childs[0];
     993                                        stack[depth++]=node->childs[1];
    760994                                }
    761995                                else
    762996                                {
    763                                 if(p.b->isinternal())
    764                                         {
    765                                         stack[depth++]=sStkNN(p.a,p.b->childs[0]);
    766                                         stack[depth++]=sStkNN(p.a,p.b->childs[1]);
    767                                         }
    768                                         else
    769                                         {
    770                                         policy.Process(p.a,p.b);
    771                                         }
     997                                        policy.Process(node);
    772998                                }
    773999                        }
    … …  
    7781004//
    7791005DBVT_PREFIX
    780 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
    781                                                                         const btDbvtNode* root1,
    782                                                                         const btTransform& xform,
    783                                                                         DBVT_IPOLICY)
    784 {
    785 DBVT_CHECKTYPE
    786 if(root0&&root1)
    787         {
    788         btAlignedObjectArray<sStkNN>    stack;
    789         int                                                             depth=1;
    790         int                                                             treshold=DOUBLE_STACKSIZE-4;
    791         stack.resize(DOUBLE_STACKSIZE);
    792         stack[0]=sStkNN(root0,root1);
    793         do      {
    794                 sStkNN  p=stack[--depth];
    795                 if(Intersect(p.a->volume,p.b->volume,xform))
    796                         {
    797                         if(depth>treshold)
    798                                 {
    799                                 stack.resize(stack.size()*2);
    800                                 treshold=stack.size()-4;
    801                                 }
    802                         if(p.a->isinternal())
    803                                 {
    804                                 if(p.b->isinternal())
    805                                         {                                       
    806                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
    807                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
    808                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
    809                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
     1006inline void             btDbvt::rayTest(        const btDbvtNode* root,
     1007                                                                const btVector3& rayFrom,
     1008                                                                const btVector3& rayTo,
     1009                                                                DBVT_IPOLICY)
     1010{
     1011        DBVT_CHECKTYPE
     1012                if(root)
     1013                {
     1014                        btVector3 rayDir = (rayTo-rayFrom);
     1015                        rayDir.normalize ();
     1016
     1017                        ///what about division by zero? --> just set rayDirection[i] to INF/1e30
     1018                        btVector3 rayDirectionInverse;
     1019                        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
     1020                        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
     1021                        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
     1022                        unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
     1023
     1024                        btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
     1025
     1026                        btVector3 resultNormal;
     1027
     1028                        btAlignedObjectArray<const btDbvtNode*> stack;
     1029
     1030                        int                                                             depth=1;
     1031                        int                                                             treshold=DOUBLE_STACKSIZE-2;
     1032
     1033                        stack.resize(DOUBLE_STACKSIZE);
     1034                        stack[0]=root;
     1035                        btVector3 bounds[2];
     1036                        do      {
     1037                                const btDbvtNode*       node=stack[--depth];
     1038
     1039                                bounds[0] = node->volume.Mins();
     1040                                bounds[1] = node->volume.Maxs();
     1041                               
     1042                                btScalar tmin=1.f,lambda_min=0.f;
     1043                                unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
     1044
     1045#ifdef COMPARE_BTRAY_AABB2
     1046                                btScalar param=1.f;
     1047                                bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
     1048                                btAssert(result1 == result2);
     1049#endif //TEST_BTRAY_AABB2
     1050
     1051                                if(result1)
     1052                                {
     1053                                        if(node->isinternal())
     1054                                        {
     1055                                                if(depth>treshold)
     1056                                                {
     1057                                                        stack.resize(stack.size()*2);
     1058                                                        treshold=stack.size()-2;
     1059                                                }
     1060                                                stack[depth++]=node->childs[0];
     1061                                                stack[depth++]=node->childs[1];
    8101062                                        }
    8111063                                        else
    8121064                                        {
    813                                         stack[depth++]=sStkNN(p.a->childs[0],p.b);
    814                                         stack[depth++]=sStkNN(p.a->childs[1],p.b);
    815                                         }
    816                                 }
    817                                 else
    818                                 {
    819                                 if(p.b->isinternal())
    820                                         {
    821                                         stack[depth++]=sStkNN(p.a,p.b->childs[0]);
    822                                         stack[depth++]=sStkNN(p.a,p.b->childs[1]);
    823                                         }
    824                                         else
    825                                         {
    826                                         policy.Process(p.a,p.b);
    827                                         }
    828                                 }
    829                         }
    830                 } while(depth);
    831         }
    832 }
    833 
    834 //
    835 DBVT_PREFIX
    836 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
    837                                                                         const btTransform& xform0,
    838                                                                         const btDbvtNode* root1,
    839                                                                         const btTransform& xform1,
    840                                                                         DBVT_IPOLICY)
    841 {
    842 const btTransform       xform=xform0.inverse()*xform1;
    843 collideTT(root0,root1,xform,policy);
    844 }
    845 
    846 //
    847 DBVT_PREFIX
    848 inline void             btDbvt::collideTV(      const btDbvtNode* root,
    849                                                                         const btDbvtVolume& vol,
    850                                                                         DBVT_IPOLICY)
    851 {
    852 DBVT_CHECKTYPE
    853 if(root)
    854         {
    855         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
    856         btAlignedObjectArray<const btDbvtNode*> stack;
    857         stack.reserve(SIMPLE_STACKSIZE);
    858         stack.push_back(root);
    859         do      {
    860                 const btDbvtNode*       n=stack[stack.size()-1];
    861                 stack.pop_back();
    862                 if(Intersect(n->volume,volume))
    863                         {
    864                         if(n->isinternal())
    865                                 {
    866                                 stack.push_back(n->childs[0]);
    867                                 stack.push_back(n->childs[1]);
    868                                 }
    869                                 else
    870                                 {
    871                                 policy.Process(n);
    872                                 }
    873                         }
    874                 } while(stack.size()>0);
    875         }
    876 }
    877 
    878 //
    879 DBVT_PREFIX
    880 inline void             btDbvt::collideRAY(     const btDbvtNode* root,
    881                                                                         const btVector3& origin,
    882                                                                         const btVector3& direction,
    883                                                                         DBVT_IPOLICY)
    884 {
    885 DBVT_CHECKTYPE
    886 if(root)
    887         {
    888         const btVector3 normal=direction.normalized();
    889         const btVector3 invdir( 1/normal.x(),
    890                                                         1/normal.y(),
    891                                                         1/normal.z());
    892         const unsigned  signs[]={       direction.x()<0,
    893                                                                 direction.y()<0,
    894                                                                 direction.z()<0};
    895         btAlignedObjectArray<const btDbvtNode*> stack;
    896         stack.reserve(SIMPLE_STACKSIZE);
    897         stack.push_back(root);
    898         do      {
    899                 const btDbvtNode*       node=stack[stack.size()-1];
    900                 stack.pop_back();
    901                 if(Intersect(node->volume,origin,invdir,signs))
    902                         {
    903                         if(node->isinternal())
    904                                 {
    905                                 stack.push_back(node->childs[0]);
    906                                 stack.push_back(node->childs[1]);
    907                                 }
    908                                 else
    909                                 {
    910                                 policy.Process(node);
    911                                 }
    912                         }
    913                 } while(stack.size());
    914         }
     1065                                                policy.Process(node);
     1066                                        }
     1067                                }
     1068                        } while(depth);
     1069
     1070                }
    9151071}
    9161072
    … …  
    9231079                                                                        DBVT_IPOLICY)
    9241080{
    925 DBVT_CHECKTYPE
    926 if(root)
    927         {
    928         const int                                               inside=(1<<count)-1;
    929         btAlignedObjectArray<sStkNP>    stack;
    930         int                                                             signs[sizeof(unsigned)*8];
    931         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
    932         for(int i=0;i<count;++i)
     1081        DBVT_CHECKTYPE
     1082                if(root)
    9331083                {
    934                 signs[i]=       ((normals[i].x()>=0)?1:0)+
     1084                        const int                                               inside=(1<<count)-1;
     1085                        btAlignedObjectArray<sStkNP>    stack;
     1086                        int                                                             signs[sizeof(unsigned)*8];
     1087                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
     1088                        for(int i=0;i<count;++i)
     1089                        {
     1090                                signs[i]=       ((normals[i].x()>=0)?1:0)+
    9351091                                        ((normals[i].y()>=0)?2:0)+
    9361092                                        ((normals[i].z()>=0)?4:0);
     1093                        }
     1094                        stack.reserve(SIMPLE_STACKSIZE);
     1095                        stack.push_back(sStkNP(root,0));
     1096                        do      {
     1097                                sStkNP  se=stack[stack.size()-1];
     1098                                bool    out=false;
     1099                                stack.pop_back();
     1100                                for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
     1101                                {
     1102                                        if(0==(se.mask&j))
     1103                                        {
     1104                                                const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
     1105                                                switch(side)
     1106                                                {
     1107                                                case    -1:     out=true;break;
     1108                                                case    +1:     se.mask|=j;break;
     1109                                                }
     1110                                        }
     1111                                }
     1112                                if(!out)
     1113                                {
     1114                                        if((se.mask!=inside)&&(se.node->isinternal()))
     1115                                        {
     1116                                                stack.push_back(sStkNP(se.node->childs[0],se.mask));
     1117                                                stack.push_back(sStkNP(se.node->childs[1],se.mask));
     1118                                        }
     1119                                        else
     1120                                        {
     1121                                                if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
     1122                                        }
     1123                                }
     1124                        } while(stack.size());
    9371125                }
    938         stack.reserve(SIMPLE_STACKSIZE);
    939         stack.push_back(sStkNP(root,0));
    940         do      {
    941                 sStkNP  se=stack[stack.size()-1];
    942                 bool    out=false;
    943                 stack.pop_back();
    944                 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
    945                         {
    946                         if(0==(se.mask&j))
    947                                 {
    948                                 const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
    949                                 switch(side)
    950                                         {
    951                                         case    -1:     out=true;break;
    952                                         case    +1:     se.mask|=j;break;
    953                                         }
    954                                 }
    955                         }
    956                 if(!out)
    957                         {
    958                         if((se.mask!=inside)&&(se.node->isinternal()))
    959                                 {
    960                                 stack.push_back(sStkNP(se.node->childs[0],se.mask));
    961                                 stack.push_back(sStkNP(se.node->childs[1],se.mask));
    962                                 }
    963                                 else
    964                                 {
    965                                 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
    966                                 }
    967                         }
    968                 } while(stack.size());
    969         }
    9701126}
    9711127
    … …  
    9731129DBVT_PREFIX
    9741130inline void             btDbvt::collideOCL(     const btDbvtNode* root,
    975                                                                         const btVector3* normals,
    976                                                                         const btScalar* offsets,
    977                                                                         const btVector3& sortaxis,
    978                                                                         int count,
    979                                                                         DBVT_IPOLICY,
    980                                                                         bool fsort)
    981 {
    982 DBVT_CHECKTYPE
    983 if(root)
    984         {
    985         const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
    986                                                                                         (sortaxis[1]>=0?2:0)+
    987                                                                                         (sortaxis[2]>=0?4:0);
    988         const int                                               inside=(1<<count)-1;
    989         btAlignedObjectArray<sStkNPS>   stock;
    990         btAlignedObjectArray<int>               ifree;
    991         btAlignedObjectArray<int>               stack;
    992         int                                                             signs[sizeof(unsigned)*8];
    993         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
    994         for(int i=0;i<count;++i)
     1131                                                                   const btVector3* normals,
     1132                                                                   const btScalar* offsets,
     1133                                                                   const btVector3& sortaxis,
     1134                                                                   int count,
     1135                                                                   DBVT_IPOLICY,
     1136                                                                   bool fsort)
     1137{
     1138        DBVT_CHECKTYPE
     1139                if(root)
    9951140                {
    996                 signs[i]=       ((normals[i].x()>=0)?1:0)+
     1141                        const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
     1142                                (sortaxis[1]>=0?2:0)+
     1143                                (sortaxis[2]>=0?4:0);
     1144                        const int                                               inside=(1<<count)-1;
     1145                        btAlignedObjectArray<sStkNPS>   stock;
     1146                        btAlignedObjectArray<int>               ifree;
     1147                        btAlignedObjectArray<int>               stack;
     1148                        int                                                             signs[sizeof(unsigned)*8];
     1149                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
     1150                        for(int i=0;i<count;++i)
     1151                        {
     1152                                signs[i]=       ((normals[i].x()>=0)?1:0)+
    9971153                                        ((normals[i].y()>=0)?2:0)+
    9981154                                        ((normals[i].z()>=0)?4:0);
     1155                        }
     1156                        stock.reserve(SIMPLE_STACKSIZE);
     1157                        stack.reserve(SIMPLE_STACKSIZE);
     1158                        ifree.reserve(SIMPLE_STACKSIZE);
     1159                        stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
     1160                        do      {
     1161                                const int       id=stack[stack.size()-1];
     1162                                sStkNPS         se=stock[id];
     1163                                stack.pop_back();ifree.push_back(id);
     1164                                if(se.mask!=inside)
     1165                                {
     1166                                        bool    out=false;
     1167                                        for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
     1168                                        {
     1169                                                if(0==(se.mask&j))
     1170                                                {
     1171                                                        const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
     1172                                                        switch(side)
     1173                                                        {
     1174                                                        case    -1:     out=true;break;
     1175                                                        case    +1:     se.mask|=j;break;
     1176                                                        }
     1177                                                }
     1178                                        }
     1179                                        if(out) continue;
     1180                                }
     1181                                if(policy.Descent(se.node))
     1182                                {
     1183                                        if(se.node->isinternal())
     1184                                        {
     1185                                                const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
     1186                                                sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
     1187                                                        sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
     1188                                                const int       q=nes[0].value<nes[1].value?1:0;                               
     1189                                                int                     j=stack.size();
     1190                                                if(fsort&&(j>0))
     1191                                                {
     1192                                                        /* Insert 0     */
     1193                                                        j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
     1194                                                        stack.push_back(0);
     1195#if DBVT_USE_MEMMOVE
     1196                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
     1197#else
     1198                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
     1199#endif
     1200                                                        stack[j]=allocate(ifree,stock,nes[q]);
     1201                                                        /* Insert 1     */
     1202                                                        j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
     1203                                                        stack.push_back(0);
     1204#if DBVT_USE_MEMMOVE
     1205                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
     1206#else
     1207                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
     1208#endif
     1209                                                        stack[j]=allocate(ifree,stock,nes[1-q]);
     1210                                                }
     1211                                                else
     1212                                                {
     1213                                                        stack.push_back(allocate(ifree,stock,nes[q]));
     1214                                                        stack.push_back(allocate(ifree,stock,nes[1-q]));
     1215                                                }
     1216                                        }
     1217                                        else
     1218                                        {
     1219                                                policy.Process(se.node,se.value);
     1220                                        }
     1221                                }
     1222                        } while(stack.size());
    9991223                }
    1000         stock.reserve(SIMPLE_STACKSIZE);
    1001         stack.reserve(SIMPLE_STACKSIZE);
    1002         ifree.reserve(SIMPLE_STACKSIZE);
    1003         stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
    1004         do      {
    1005                 const int       id=stack[stack.size()-1];
    1006                 sStkNPS         se=stock[id];
    1007                 stack.pop_back();ifree.push_back(id);
    1008                 if(se.mask!=inside)
    1009                         {
    1010                         bool    out=false;
    1011                         for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
    1012                                 {
    1013                                 if(0==(se.mask&j))
    1014                                         {
    1015                                         const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
    1016                                         switch(side)
    1017                                                 {
    1018                                                 case    -1:     out=true;break;
    1019                                                 case    +1:     se.mask|=j;break;
    1020                                                 }
    1021                                         }
    1022                                 }
    1023                         if(out) continue;
    1024                         }
    1025                 if(policy.Descent(se.node))
    1026                         {
    1027                         if(se.node->isinternal())
    1028                                 {
    1029                                 const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
    1030                                 sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
    1031                                                                         sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
    1032                                 const int       q=nes[0].value<nes[1].value?1:0;                               
    1033                                 int                     j=stack.size();
    1034                                 if(fsort&&(j>0))
    1035                                         {
    1036                                         /* Insert 0     */
    1037                                         j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
    1038                                         stack.push_back(0);
    1039                                         #if DBVT_USE_MEMMOVE
    1040                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
    1041                                         #else
    1042                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
    1043                                         #endif
    1044                                         stack[j]=allocate(ifree,stock,nes[q]);
    1045                                         /* Insert 1     */
    1046                                         j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
    1047                                         stack.push_back(0);
    1048                                         #if DBVT_USE_MEMMOVE
    1049                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
    1050                                         #else
    1051                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
    1052                                         #endif
    1053                                         stack[j]=allocate(ifree,stock,nes[1-q]);
    1054                                         }
    1055                                         else
    1056                                         {
    1057                                         stack.push_back(allocate(ifree,stock,nes[q]));
    1058                                         stack.push_back(allocate(ifree,stock,nes[1-q]));
    1059                                         }
    1060                                 }
    1061                                 else
    1062                                 {
    1063                                 policy.Process(se.node,se.value);
    1064                                 }
    1065                         }
    1066                 } while(stack.size());
    1067         }
    10681224}
    10691225
    … …  
    10711227DBVT_PREFIX
    10721228inline void             btDbvt::collideTU(      const btDbvtNode* root,
    1073                                                                         DBVT_IPOLICY)
    1074 {
    1075 DBVT_CHECKTYPE
    1076 if(root)
    1077         {
    1078         btAlignedObjectArray<const btDbvtNode*> stack;
    1079         stack.reserve(SIMPLE_STACKSIZE);
    1080         stack.push_back(root);
    1081         do      {
    1082                 const btDbvtNode*       n=stack[stack.size()-1];
    1083                 stack.pop_back();
    1084                 if(policy.Descent(n))
    1085                         {
    1086                         if(n->isinternal())
    1087                                 { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
    1088                                 else
    1089                                 { policy.Process(n); }
    1090                         }
    1091                 } while(stack.size()>0);
    1092         }
     1229                                                                  DBVT_IPOLICY)
     1230{
     1231        DBVT_CHECKTYPE
     1232                if(root)
     1233                {
     1234                        btAlignedObjectArray<const btDbvtNode*> stack;
     1235                        stack.reserve(SIMPLE_STACKSIZE);
     1236                        stack.push_back(root);
     1237                        do      {
     1238                                const btDbvtNode*       n=stack[stack.size()-1];
     1239                                stack.pop_back();
     1240                                if(policy.Descent(n))
     1241                                {
     1242                                        if(n->isinternal())
     1243                                        { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
     1244                                        else
     1245                                        { policy.Process(n); }
     1246                                }
     1247                        } while(stack.size()>0);
     1248                }
    10931249}
    10941250

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

    r2192 r2430  
    2727#if DBVT_BP_PROFILE
    2828struct  ProfileScope
    29         {
     29{
    3030        __forceinline ProfileScope(btClock& clock,unsigned long& value) :
    31                 m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
    32                 {
    33                 }
     31        m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
     32        {
     33        }
    3434        __forceinline ~ProfileScope()
    35                 {
     35        {
    3636                (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
    37                 }
     37        }
    3838        btClock*                m_clock;
    3939        unsigned long*  m_value;
    4040        unsigned long   m_base;
    41         };
     41};
    4242#define SPC(_value_)    ProfileScope    spc_scope(m_clock,_value_)
    4343#else
    … …  
    5353static inline void      listappend(T* item,T*& list)
    5454{
    55 item->links[0]=0;
    56 item->links[1]=list;
    57 if(list) list->links[0]=item;
    58 list=item;
     55        item->links[0]=0;
     56        item->links[1]=list;
     57        if(list) list->links[0]=item;
     58        list=item;
    5959}
    6060
    … …  
    6363static inline void      listremove(T* item,T*& list)
    6464{
    65 if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
    66 if(item->links[1]) item->links[1]->links[0]=item->links[0];
     65        if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
     66        if(item->links[1]) item->links[1]->links[0]=item->links[0];
    6767}
    6868
    … …  
    7171static inline int       listcount(T* root)
    7272{
    73 int     n=0;
    74 while(root) { ++n;root=root->links[1]; }
    75 return(n);
     73        int     n=0;
     74        while(root) { ++n;root=root->links[1]; }
     75        return(n);
    7676}
    7777
    … …  
    8080static inline void      clear(T& value)
    8181{
    82 static const struct ZeroDummy : T {} zerodummy;
    83 value=zerodummy;
     82        static const struct ZeroDummy : T {} zerodummy;
     83        value=zerodummy;
    8484}
    8585
    … …  
    9191struct  btDbvtTreeCollider : btDbvt::ICollide
    9292{
    93 btDbvtBroadphase*       pbp;
    94 btDbvtProxy*            proxy;
    95                 btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
    96 void    Process(const btDbvtNode* na,const btDbvtNode* nb)
    97         {
    98         if(na!=nb)
    99                 {
    100                 btDbvtProxy*    pa=(btDbvtProxy*)na->data;
    101                 btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
    102                 #if DBVT_BP_SORTPAIRS
    103                 if(pa>pb) btSwap(pa,pb);
    104                 #endif
    105                 pbp->m_paircache->addOverlappingPair(pa,pb);
    106                 ++pbp->m_newpairs;
    107                 }
    108         }
    109 void    Process(const btDbvtNode* n)
    110         {
    111         Process(n,proxy->leaf);
     93        btDbvtBroadphase*       pbp;
     94        btDbvtProxy*            proxy;
     95        btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
     96        void    Process(const btDbvtNode* na,const btDbvtNode* nb)
     97        {
     98                if(na!=nb)
     99                {
     100                        btDbvtProxy*    pa=(btDbvtProxy*)na->data;
     101                        btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
     102#if DBVT_BP_SORTPAIRS
     103                        if(pa>pb) btSwap(pa,pb);
     104#endif
     105                        pbp->m_paircache->addOverlappingPair(pa,pb);
     106                        ++pbp->m_newpairs;
     107                }
     108        }
     109        void    Process(const btDbvtNode* n)
     110        {
     111                Process(n,proxy->leaf);
    112112        }
    113113};
    … …  
    120120btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
    121121{
    122 m_deferedcollide        =       false;
    123 m_needcleanup           =       true;
    124 m_releasepaircache      =       (paircache!=0)?false:true;
    125 m_prediction            =       1/(btScalar)2;
    126 m_stageCurrent          =       0;
    127 m_fixedleft                     =       0;
    128 m_fupdates                      =       1;
    129 m_dupdates                      =       0;
    130 m_cupdates                      =       10;
    131 m_newpairs                      =       1;
    132 m_updates_call          =       0;
    133 m_updates_done          =       0;
    134 m_updates_ratio         =       0;
    135 m_paircache                     =       paircache?
    136                                                 paircache       :
    137                                                 new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
    138 m_gid                           =       0;
    139 m_pid                           =       0;
    140 m_cid                           =       0;
    141 for(int i=0;i<=STAGECOUNT;++i)
    142         {
    143         m_stageRoots[i]=0;
     122        m_deferedcollide        =       false;
     123        m_needcleanup           =       true;
     124        m_releasepaircache      =       (paircache!=0)?false:true;
     125        m_prediction            =       1/(btScalar)2;
     126        m_stageCurrent          =       0;
     127        m_fixedleft                     =       0;
     128        m_fupdates                      =       1;
     129        m_dupdates                      =       0;
     130        m_cupdates                      =       10;
     131        m_newpairs                      =       1;
     132        m_updates_call          =       0;
     133        m_updates_done          =       0;
     134        m_updates_ratio         =       0;
     135        m_paircache                     =       paircache?
     136paircache       :
     137        new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
     138        m_gid                           =       0;
     139        m_pid                           =       0;
     140        m_cid                           =       0;
     141        for(int i=0;i<=STAGECOUNT;++i)
     142        {
     143                m_stageRoots[i]=0;
    144144        }
    145145#if DBVT_BP_PROFILE
    146 clear(m_profiling);
     146        clear(m_profiling);
    147147#endif
    148148}
    … …  
    151151btDbvtBroadphase::~btDbvtBroadphase()
    152152{
    153 if(m_releasepaircache)
    154 {
    155         m_paircache->~btOverlappingPairCache();
    156         btAlignedFree(m_paircache);
    157 }
     153        if(m_releasepaircache)
     154        {
     155                m_paircache->~btOverlappingPairCache();
     156                btAlignedFree(m_paircache);
     157        }
    158158}
    159159
    160160//
    161161btBroadphaseProxy*                              btDbvtBroadphase::createProxy(  const btVector3& aabbMin,
    162                                                                                                                                 const btVector3& aabbMax,
    163                                                                                                                                 int /*shapeType*/,
    164                                                                                                                                 void* userPtr,
    165                                                                                                                                 short int collisionFilterGroup,
    166                                                                                                                                 short int collisionFilterMask,
    167                                                                                                                                 btDispatcher* /*dispatcher*/,
    168                                                                                                                                 void* /*multiSapProxy*/)
    169 {
    170 btDbvtProxy*            proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  userPtr,
    171                                                                                                                                                                         collisionFilterGroup,
    172                                                                                                                                                                         collisionFilterMask);
    173 proxy->aabb                     =       btDbvtVolume::FromMM(aabbMin,aabbMax);
    174 proxy->stage            =       m_stageCurrent;
    175 proxy->m_uniqueId       =       ++m_gid;
    176 proxy->leaf                     =       m_sets[0].insert(proxy->aabb,proxy);
    177 listappend(proxy,m_stageRoots[m_stageCurrent]);
    178 if(!m_deferedcollide)
    179         {
    180         btDbvtTreeCollider      collider(this);
    181         collider.proxy=proxy;
    182         btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider);
    183         btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider);
    184         }
    185 return(proxy);
     162                                                                                                                          const btVector3& aabbMax,
     163                                                                                                                          int /*shapeType*/,
     164                                                                                                                          void* userPtr,
     165                                                                                                                          short int collisionFilterGroup,
     166                                                                                                                          short int collisionFilterMask,
     167                                                                                                                          btDispatcher* /*dispatcher*/,
     168                                                                                                                          void* /*multiSapProxy*/)
     169{
     170        btDbvtProxy*            proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  aabbMin,aabbMax,userPtr,
     171                collisionFilterGroup,
     172                collisionFilterMask);
     173
     174        btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
     175
     176        //bproxy->aabb                  =       btDbvtVolume::FromMM(aabbMin,aabbMax);
     177        proxy->stage            =       m_stageCurrent;
     178        proxy->m_uniqueId       =       ++m_gid;
     179        proxy->leaf                     =       m_sets[0].insert(aabb,proxy);
     180        listappend(proxy,m_stageRoots[m_stageCurrent]);
     181        if(!m_deferedcollide)
     182        {
     183                btDbvtTreeCollider      collider(this);
     184                collider.proxy=proxy;
     185                m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
     186                m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
     187        }
     188        return(proxy);
    186189}
    187190
    188191//
    189192void                                                    btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
    190                                                                                                                                 btDispatcher* dispatcher)
    191 {
    192 btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
    193 if(proxy->stage==STAGECOUNT)
    194         m_sets[1].remove(proxy->leaf);
     193                                                                                                                           btDispatcher* dispatcher)
     194{
     195        btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
     196        if(proxy->stage==STAGECOUNT)
     197                m_sets[1].remove(proxy->leaf);
    195198        else
    196         m_sets[0].remove(proxy->leaf);
    197 listremove(proxy,m_stageRoots[proxy->stage]);
    198 m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
    199 btAlignedFree(proxy);
    200 m_needcleanup=true;
     199                m_sets[0].remove(proxy->leaf);
     200        listremove(proxy,m_stageRoots[proxy->stage]);
     201        m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
     202        btAlignedFree(proxy);
     203        m_needcleanup=true;
     204}
     205
     206void    btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
     207{
     208        btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
     209        aabbMin = proxy->m_aabbMin;
     210        aabbMax = proxy->m_aabbMax;
     211}
     212
     213void    btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
     214{
     215
     216        struct  BroadphaseRayTester : btDbvt::ICollide
     217        {
     218                btBroadphaseRayCallback& m_rayCallback;
     219                BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
     220                        :m_rayCallback(orgCallback)
     221                {
     222                }
     223                void                                    Process(const btDbvtNode* leaf)
     224                {
     225                        btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
     226                        m_rayCallback.process(proxy);
     227                }
     228        };     
     229
     230        BroadphaseRayTester callback(rayCallback);
     231
     232        m_sets[0].rayTestInternal(      m_sets[0].m_root,
     233                rayFrom,
     234                rayTo,
     235                rayCallback.m_rayDirectionInverse,
     236                rayCallback.m_signs,
     237                rayCallback.m_lambda_max,
     238                aabbMin,
     239                aabbMax,
     240                callback);
     241
     242        m_sets[1].rayTestInternal(      m_sets[1].m_root,
     243                rayFrom,
     244                rayTo,
     245                rayCallback.m_rayDirectionInverse,
     246                rayCallback.m_signs,
     247                rayCallback.m_lambda_max,
     248                aabbMin,
     249                aabbMax,
     250                callback);
     251
    201252}
    202253
    203254//
    204255void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
    205                                                                                                                                 const btVector3& aabbMin,
    206                                                                                                                                 const btVector3& aabbMax,
    207                                                                                                                                 btDispatcher* /*dispatcher*/)
    208 {
    209 btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
    210 ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
     256                                                                                                                  const btVector3& aabbMin,
     257                                                                                                                  const btVector3& aabbMax,
     258                                                                                                                  btDispatcher* /*dispatcher*/)
     259{
     260        btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
     261        ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
    211262#if DBVT_BP_PREVENTFALSEUPDATE
    212 if(NotEqual(aabb,proxy->leaf->volume))
    213 #endif
    214         {
    215         bool    docollide=false;
    216         if(proxy->stage==STAGECOUNT)
     263        if(NotEqual(aabb,proxy->leaf->volume))
     264#endif
     265        {
     266                bool    docollide=false;
     267                if(proxy->stage==STAGECOUNT)
    217268                {/* fixed -> dynamic set        */
    218                 m_sets[1].remove(proxy->leaf);
    219                 proxy->leaf=m_sets[0].insert(aabb,proxy);
    220                 docollide=true;
     269                        m_sets[1].remove(proxy->leaf);
     270                        proxy->leaf=m_sets[0].insert(aabb,proxy);
     271                        docollide=true;
    221272                }
    222273                else
    223274                {/* dynamic set                         */
    224                 ++m_updates_call;
    225                 if(Intersect(proxy->leaf->volume,aabb))
     275                        ++m_updates_call;
     276                        if(Intersect(proxy->leaf->volume,aabb))
    226277                        {/* Moving                              */
    227                         const btVector3 delta=aabbMin-proxy->aabb.Mins();
    228                         btVector3               velocity(aabb.Extents()*m_prediction);
    229                         if(delta[0]<0) velocity[0]=-velocity[0];
    230                         if(delta[1]<0) velocity[1]=-velocity[1];
    231                         if(delta[2]<0) velocity[2]=-velocity[2];
    232                         if      (
    233                                 #ifdef DBVT_BP_MARGIN                           
    234                                 m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
    235                                 #else
    236                                 m_sets[0].update(proxy->leaf,aabb,velocity)
    237                                 #endif
    238                                 )
     278
     279                                const btVector3 delta=aabbMin-proxy->m_aabbMin;
     280                                btVector3               velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
     281                                if(delta[0]<0) velocity[0]=-velocity[0];
     282                                if(delta[1]<0) velocity[1]=-velocity[1];
     283                                if(delta[2]<0) velocity[2]=-velocity[2];
     284                                if      (
     285#ifdef DBVT_BP_MARGIN                           
     286                                        m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
     287#else
     288                                        m_sets[0].update(proxy->leaf,aabb,velocity)
     289#endif
     290                                        )
    239291                                {
    240                                 ++m_updates_done;
    241                                 docollide=true;
     292                                        ++m_updates_done;
     293                                        docollide=true;
    242294                                }
    243295                        }
    244296                        else
    245297                        {/* Teleporting                 */
    246                         m_sets[0].update(proxy->leaf,aabb);
    247                         ++m_updates_done;
    248                         docollide=true;
     298                                m_sets[0].update(proxy->leaf,aabb);
     299                                ++m_updates_done;
     300                                docollide=true;
    249301                        }       
    250302                }
    251         listremove(proxy,m_stageRoots[proxy->stage]);
    252         proxy->aabb             =       aabb;
    253         proxy->stage    =       m_stageCurrent;
    254         listappend(proxy,m_stageRoots[m_stageCurrent]);
    255         if(docollide)
    256                 {
    257                 m_needcleanup=true;
    258                 if(!m_deferedcollide)
     303                listremove(proxy,m_stageRoots[proxy->stage]);
     304                proxy->m_aabbMin = aabbMin;
     305                proxy->m_aabbMax = aabbMax;
     306                proxy->stage    =       m_stageCurrent;
     307                listappend(proxy,m_stageRoots[m_stageCurrent]);
     308                if(docollide)
     309                {
     310                        m_needcleanup=true;
     311                        if(!m_deferedcollide)
    259312                        {
    260                         btDbvtTreeCollider      collider(this);
    261                         btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
    262                         btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
     313                                btDbvtTreeCollider      collider(this);
     314                                m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
     315                                m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
    263316                        }
    264317                }       
    … …  
    269322void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
    270323{
    271 collide(dispatcher);
     324        collide(dispatcher);
    272325#if DBVT_BP_PROFILE
    273 if(0==(m_pid%DBVT_BP_PROFILING_RATE))
     326        if(0==(m_pid%DBVT_BP_PROFILING_RATE))
    274327        {       
    275         printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
    276         unsigned int    total=m_profiling.m_total;
    277         if(total<=0) total=1;
    278         printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
    279         printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
    280         printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
    281         printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
    282         const unsigned long     sum=m_profiling.m_ddcollide+
    283                                                         m_profiling.m_fdcollide+
    284                                                         m_profiling.m_cleanup;
    285         printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
    286         printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
    287         clear(m_profiling);
    288         m_clock.reset();
     328                printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
     329                unsigned int    total=m_profiling.m_total;
     330                if(total<=0) total=1;
     331                printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
     332                printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
     333                printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
     334                printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
     335                const unsigned long     sum=m_profiling.m_ddcollide+
     336                        m_profiling.m_fdcollide+
     337                        m_profiling.m_cleanup;
     338                printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
     339                printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
     340                clear(m_profiling);
     341                m_clock.reset();
    289342        }
    290343#endif
    … …  
    294347void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
    295348{
    296 SPC(m_profiling.m_total);
    297 /* optimize                             */
    298 m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
    299 if(m_fixedleft)
    300         {
    301         const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
    302         m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
    303         m_fixedleft=btMax<int>(0,m_fixedleft-count);
    304         }
    305 /* dynamic -> fixed set */
    306 m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
    307 btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
    308 if(current)
    309         {
    310         btDbvtTreeCollider      collider(this);
    311         do      {
    312                 btDbvtProxy*    next=current->links[1];
    313                 listremove(current,m_stageRoots[current->stage]);
    314                 listappend(current,m_stageRoots[STAGECOUNT]);
    315                 #if DBVT_BP_ACCURATESLEEPING
    316                 m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
    317                 collider.proxy=current;
    318                 btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
    319                 btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
    320                 #endif
    321                 m_sets[0].remove(current->leaf);
    322                 current->leaf   =       m_sets[1].insert(current->aabb,current);
    323                 current->stage  =       STAGECOUNT;     
    324                 current                 =       next;
     349        SPC(m_profiling.m_total);
     350        /* optimize                             */
     351        m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
     352        if(m_fixedleft)
     353        {
     354                const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
     355                m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
     356                m_fixedleft=btMax<int>(0,m_fixedleft-count);
     357        }
     358        /* dynamic -> fixed set */
     359        m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
     360        btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
     361        if(current)
     362        {
     363                btDbvtTreeCollider      collider(this);
     364                do      {
     365                        btDbvtProxy*    next=current->links[1];
     366                        listremove(current,m_stageRoots[current->stage]);
     367                        listappend(current,m_stageRoots[STAGECOUNT]);
     368#if DBVT_BP_ACCURATESLEEPING
     369                        m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
     370                        collider.proxy=current;
     371                        btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
     372                        btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
     373#endif
     374                        m_sets[0].remove(current->leaf);
     375                        ATTRIBUTE_ALIGNED16(btDbvtVolume)       curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
     376                        current->leaf   =       m_sets[1].insert(curAabb,current);
     377                        current->stage  =       STAGECOUNT;     
     378                        current                 =       next;
    325379                } while(current);
    326         m_fixedleft=m_sets[1].m_leaves;
    327         m_needcleanup=true;
    328         }
    329 /* collide dynamics             */
    330         {
    331         btDbvtTreeCollider      collider(this);
    332         if(m_deferedcollide)
    333                 {
    334                 SPC(m_profiling.m_fdcollide);
    335                 btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
    336                 }
    337         if(m_deferedcollide)
    338                 {
    339                 SPC(m_profiling.m_ddcollide);
    340                 btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
    341                 }
    342         }
    343 /* clean up                             */
    344 if(m_needcleanup)
    345         {
    346         SPC(m_profiling.m_cleanup);
    347         btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
    348         if(pairs.size()>0)
    349                 {
    350                 const int       ci=pairs.size();
    351                 int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
    352                 for(int i=0;i<ni;++i)
     380                m_fixedleft=m_sets[1].m_leaves;
     381                m_needcleanup=true;
     382        }
     383        /* collide dynamics             */
     384        {
     385                btDbvtTreeCollider      collider(this);
     386                if(m_deferedcollide)
     387                {
     388                        SPC(m_profiling.m_fdcollide);
     389                        m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
     390                }
     391                if(m_deferedcollide)
     392                {
     393                        SPC(m_profiling.m_ddcollide);
     394                        m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
     395                }
     396        }
     397        /* clean up                             */
     398        if(m_needcleanup)
     399        {
     400                SPC(m_profiling.m_cleanup);
     401                btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
     402                if(pairs.size()>0)
     403                {
     404                        const int       ci=pairs.size();
     405                        int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
     406                        for(int i=0;i<ni;++i)
    353407                        {
    354                         btBroadphasePair&       p=pairs[(m_cid+i)%ci];
    355                         btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
    356                         btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
    357                         if(!Intersect(pa->leaf->volume,pb->leaf->volume))
     408                                btBroadphasePair&       p=pairs[(m_cid+i)%ci];
     409                                btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
     410                                btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
     411                                if(!Intersect(pa->leaf->volume,pb->leaf->volume))
    358412                                {
    359                                 #if DBVT_BP_SORTPAIRS
    360                                 if(pa>pb) btSwap(pa,pb);
    361                                 #endif
    362                                 m_paircache->removeOverlappingPair(pa,pb,dispatcher);
    363                                 --ni;--i;
     413#if DBVT_BP_SORTPAIRS
     414                                        if(pa>pb) btSwap(pa,pb);
     415#endif
     416                                        m_paircache->removeOverlappingPair(pa,pb,dispatcher);
     417                                        --ni;--i;
    364418                                }
    365419                        }
    366                 if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
    367                 }
    368         }
    369 ++m_pid;
    370 m_newpairs=1;
    371 m_needcleanup=false;
    372 if(m_updates_call>0)
     420                        if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
     421                }
     422        }
     423        ++m_pid;
     424        m_newpairs=1;
     425        m_needcleanup=false;
     426        if(m_updates_call>0)
    373427        { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
    374428        else
    375429        { m_updates_ratio=0; }
    376 m_updates_done/=2;
    377 m_updates_call/=2;
     430        m_updates_done/=2;
     431        m_updates_call/=2;
    378432}
    379433
    … …  
    381435void                                                    btDbvtBroadphase::optimize()
    382436{
    383 m_sets[0].optimizeTopDown();
    384 m_sets[1].optimizeTopDown();
     437        m_sets[0].optimizeTopDown();
     438        m_sets[1].optimizeTopDown();
    385439}
    386440
    … …  
    388442btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
    389443{
    390 return(m_paircache);
     444        return(m_paircache);
    391445}
    392446
    … …  
    394448const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
    395449{
    396 return(m_paircache);
     450        return(m_paircache);
    397451}
    398452
    … …  
    403457        ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
    404458
    405 if(!m_sets[0].empty())
    406         if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
    407                                                                         m_sets[1].m_root->volume,bounds);
    408                                                         else
    409                                                         bounds=m_sets[0].m_root->volume;
    410 else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
    411                                                         else
    412                                                         bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
    413 aabbMin=bounds.Mins();
    414 aabbMax=bounds.Maxs();
     459        if(!m_sets[0].empty())
     460                if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
     461                        m_sets[1].m_root->volume,bounds);
     462                else
     463                        bounds=m_sets[0].m_root->volume;
     464        else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
     465        else
     466                bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
     467        aabbMin=bounds.Mins();
     468        aabbMax=bounds.Maxs();
    415469}
    416470
    … …  
    423477
    424478struct  btBroadphaseBenchmark
    425         {
     479{
    426480        struct  Experiment
    427                 {
     481        {
    428482                const char*                     name;
    429483                int                                     object_count;
    … …  
    433487                btScalar                        speed;
    434488                btScalar                        amplitude;
    435                 };
     489        };
    436490        struct  Object
    437                 {
     491        {
    438492                btVector3                       center;
    439493                btVector3                       extents;
    … …  
    441495                btScalar                        time;
    442496                void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
    443                         {
     497                {
    444498                        time            +=      speed;
    445499                        center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
    446                                                         btSin(time)*amplitude/2;
     500                                btSin(time)*amplitude/2;
    447501                        center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
    448                                                         btSin(time)*amplitude;
     502                                btSin(time)*amplitude;
    449503                        center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
    450504                        pbi->setAabb(proxy,center-extents,center+extents,0);
    451                         }
    452                 };
     505                }
     506        };
    453507        static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
    454508        static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
    455509        static void             OutputTime(const char* name,btClock& c,unsigned count=0)
    456                 {
     510        {
    457511                const unsigned long     us=c.getTimeMicroseconds();
    458512                const unsigned long     ms=(us+500)/1000;
    … …  
    460514                if(count>0)
    461515                        printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
    462                         else
     516                else
    463517                        printf("%s : %u us (%u ms)\r\n",name,us,ms);
    464                 }
     518        }
     519};
     520
     521void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
     522{
     523        static const btBroadphaseBenchmark::Experiment          experiments[]=
     524        {
     525                {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
     526                /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
     527                {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
    465528        };
    466 
    467 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
    468 {
    469 static const btBroadphaseBenchmark::Experiment          experiments[]=
    470         {
    471         {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
    472         /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
    473         {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
    474         };
    475 static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
    476 btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
    477 btClock                                                                                                 wallclock;
    478 /* Begin                        */
    479 for(int iexp=0;iexp<nexperiments;++iexp)
    480         {
    481         const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
    482         const int                                                                       object_count=experiment.object_count;
    483         const int                                                                       update_count=(object_count*experiment.update_count)/100;
    484         const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
    485         const btScalar                                                          speed=experiment.speed;
    486         const btScalar                                                          amplitude=experiment.amplitude;
    487         printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
    488         printf("\tObjects: %u\r\n",object_count);
    489         printf("\tUpdate: %u\r\n",update_count);
    490         printf("\tSpawn: %u\r\n",spawn_count);
    491         printf("\tSpeed: %f\r\n",speed);
    492         printf("\tAmplitude: %f\r\n",amplitude);
    493         srand(180673);
    494         /* Create objects       */
    495         wallclock.reset();
    496         objects.reserve(object_count);
    497         for(int i=0;i<object_count;++i)
    498                 {
    499                 btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
    500                 po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
    501                 po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
    502                 po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
    503                 po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
    504                 po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
    505                 po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
    506                 po->time=btBroadphaseBenchmark::UnitRand()*2000;
    507                 po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
    508                 objects.push_back(po);
    509                 }
    510         btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
    511         /* First update         */
    512         wallclock.reset();
    513         for(int i=0;i<objects.size();++i)
    514                 {
    515                 objects[i]->update(speed,amplitude,pbi);
    516                 }
    517         btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
    518         /* Updates                      */
    519         wallclock.reset();
    520         for(int i=0;i<experiment.iterations;++i)
    521                 {
    522                 for(int j=0;j<update_count;++j)
     529        static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
     530        btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
     531        btClock                                                                                                 wallclock;
     532        /* Begin                        */
     533        for(int iexp=0;iexp<nexperiments;++iexp)
     534        {
     535                const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
     536                const int                                                                       object_count=experiment.object_count;
     537                const int                                                                       update_count=(object_count*experiment.update_count)/100;
     538                const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
     539                const btScalar                                                          speed=experiment.speed;
     540                const btScalar                                                          amplitude=experiment.amplitude;
     541                printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
     542                printf("\tObjects: %u\r\n",object_count);
     543                printf("\tUpdate: %u\r\n",update_count);
     544                printf("\tSpawn: %u\r\n",spawn_count);
     545                printf("\tSpeed: %f\r\n",speed);
     546                printf("\tAmplitude: %f\r\n",amplitude);
     547                srand(180673);
     548                /* Create objects       */
     549                wallclock.reset();
     550                objects.reserve(object_count);
     551                for(int i=0;i<object_count;++i)
     552                {
     553                        btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
     554                        po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
     555                        po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
     556                        po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
     557                        po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
     558                        po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
     559                        po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
     560                        po->time=btBroadphaseBenchmark::UnitRand()*2000;
     561                        po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
     562                        objects.push_back(po);
     563                }
     564                btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
     565                /* First update         */
     566                wallclock.reset();
     567                for(int i=0;i<objects.size();++i)
     568                {
     569                        objects[i]->update(speed,amplitude,pbi);
     570                }
     571                btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
     572                /* Updates                      */
     573                wallclock.reset();
     574                for(int i=0;i<experiment.iterations;++i)
     575                {
     576                        for(int j=0;j<update_count;++j)
    523577                        {                               
    524                         objects[j]->update(speed,amplitude,pbi);
     578                                objects[j]->update(speed,amplitude,pbi);
    525579                        }
    526                 pbi->calculateOverlappingPairs(0);
    527                 }
    528         btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
    529         /* Clean up                     */
    530         wallclock.reset();
    531         for(int i=0;i<objects.size();++i)
    532                 {
    533                 pbi->destroyProxy(objects[i]->proxy,0);
    534                 delete objects[i];
    535                 }
    536         objects.resize(0);
    537         btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
     580                        pbi->calculateOverlappingPairs(0);
     581                }
     582                btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
     583                /* Clean up                     */
     584                wallclock.reset();
     585                for(int i=0;i<objects.size();++i)
     586                {
     587                        pbi->destroyProxy(objects[i]->proxy,0);
     588                        delete objects[i];
     589                }
     590                objects.resize(0);
     591                btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
    538592        }
    539593

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

    r2192 r2430  
    3232
    3333#if DBVT_BP_PROFILE
    34         #define DBVT_BP_PROFILING_RATE  256
    35         #include "LinearMath/btQuickprof.h"
     34#define DBVT_BP_PROFILING_RATE  256
     35#include "LinearMath/btQuickprof.h"
    3636#endif
    3737
    … …  
    4141struct btDbvtProxy : btBroadphaseProxy
    4242{
    43 /* Fields               */
    44 btDbvtAabbMm    aabb;
    45 btDbvtNode*             leaf;
    46 btDbvtProxy*    links[2];
    47 int                             stage;
    48 /* ctor                 */
    49 btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
    50         btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask)
     43        /* Fields               */
     44        //btDbvtAabbMm  aabb;
     45        btDbvtNode*             leaf;
     46        btDbvtProxy*    links[2];
     47        int                             stage;
     48        /* ctor                 */
     49        btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
     50        btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
    5151        {
    52         links[0]=links[1]=0;
     52                links[0]=links[1]=0;
    5353        }
    5454};
    … …  
    6161struct  btDbvtBroadphase : btBroadphaseInterface
    6262{
    63 /* Config               */
    64 enum    {
     63        /* Config               */
     64        enum    {
    6565                DYNAMIC_SET                     =       0,      /* Dynamic set index    */
    6666                FIXED_SET                       =       1,      /* Fixed set index              */
    6767                STAGECOUNT                      =       2       /* Number of stages             */
    68                 };
    69 /* Fields               */
    70 btDbvt                                  m_sets[2];                                      // Dbvt sets
    71 btDbvtProxy*                    m_stageRoots[STAGECOUNT+1];     // Stages list
    72 btOverlappingPairCache* m_paircache;                            // Pair cache
    73 btScalar                                m_prediction;                           // Velocity prediction
    74 int                                             m_stageCurrent;                         // Current stage
    75 int                                             m_fupdates;                                     // % of fixed updates per frame
    76 int                                             m_dupdates;                                     // % of dynamic updates per frame
    77 int                                             m_cupdates;                                     // % of cleanup updates per frame
    78 int                                             m_newpairs;                                     // Number of pairs created
    79 int                                             m_fixedleft;                            // Fixed optimization left
    80 unsigned                                m_updates_call;                         // Number of updates call
    81 unsigned                                m_updates_done;                         // Number of updates done
    82 btScalar                                m_updates_ratio;                        // m_updates_done/m_updates_call
    83 int                                             m_pid;                                          // Parse id
    84 int                                             m_cid;                                          // Cleanup index
    85 int                                             m_gid;                                          // Gen id
    86 bool                                    m_releasepaircache;                     // Release pair cache on delete
    87 bool                                    m_deferedcollide;                       // Defere dynamic/static collision to collide call
    88 bool                                    m_needcleanup;                          // Need to run cleanup?
     68        };
     69        /* Fields               */
     70        btDbvt                                  m_sets[2];                                      // Dbvt sets
     71        btDbvtProxy*                    m_stageRoots[STAGECOUNT+1];     // Stages list
     72        btOverlappingPairCache* m_paircache;                            // Pair cache
     73        btScalar                                m_prediction;                           // Velocity prediction
     74        int                                             m_stageCurrent;                         // Current stage
     75        int                                             m_fupdates;                                     // % of fixed updates per frame
     76        int                                             m_dupdates;                                     // % of dynamic updates per frame
     77        int                                             m_cupdates;                                     // % of cleanup updates per frame
     78        int                                             m_newpairs;                                     // Number of pairs created
     79        int                                             m_fixedleft;                            // Fixed optimization left
     80        unsigned                                m_updates_call;                         // Number of updates call
     81        unsigned                                m_updates_done;                         // Number of updates done
     82        btScalar                                m_updates_ratio;                        // m_updates_done/m_updates_call
     83        int                                             m_pid;                                          // Parse id
     84        int                                             m_cid;                                          // Cleanup index
     85        int                                             m_gid;                                          // Gen id
     86        bool                                    m_releasepaircache;                     // Release pair cache on delete
     87        bool                                    m_deferedcollide;                       // Defere dynamic/static collision to collide call
     88        bool                                    m_needcleanup;                          // Need to run cleanup?
    8989#if DBVT_BP_PROFILE
    90 btClock                                 m_clock;
    91 struct  {
     90        btClock                                 m_clock;
     91        struct  {
    9292                unsigned long           m_total;
    9393                unsigned long           m_ddcollide;
    … …  
    9595                unsigned long           m_cleanup;
    9696                unsigned long           m_jobcount;
    97                 }                               m_profiling;
     97        }                               m_profiling;
    9898#endif
    99 /* Methods              */
    100 btDbvtBroadphase(btOverlappingPairCache* paircache=0);
    101 ~btDbvtBroadphase();
    102 void                                                    collide(btDispatcher* dispatcher);
    103 void                                                    optimize();
    104 /* btBroadphaseInterface Implementation */
    105 btBroadphaseProxy*                              createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
    106 void                                                    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    107 void                                                    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
    108 void                                                    calculateOverlappingPairs(btDispatcher* dispatcher);
    109 btOverlappingPairCache*                 getOverlappingPairCache();
    110 const btOverlappingPairCache*   getOverlappingPairCache() const;
    111 void                                                    getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
    112 void                                                    printStats();
    113 static void                                             benchmark(btBroadphaseInterface*);
     99        /* Methods              */
     100        btDbvtBroadphase(btOverlappingPairCache* paircache=0);
     101        ~btDbvtBroadphase();
     102        void                                                    collide(btDispatcher* dispatcher);
     103        void                                                    optimize();
     104        /* btBroadphaseInterface Implementation */
     105        btBroadphaseProxy*                              createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
     106        void                                                    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
     107        void                                                    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
     108        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));
     109
     110        virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
     111        void                                                    calculateOverlappingPairs(btDispatcher* dispatcher);
     112        btOverlappingPairCache*                 getOverlappingPairCache();
     113        const btOverlappingPairCache*   getOverlappingPairCache() const;
     114        void                                                    getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
     115        void                                                    printStats();
     116        static void                                             benchmark(btBroadphaseInterface*);
    114117};
    115118

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h

    r2192 r2430  
    4747                m_useEpa(true),
    4848                m_allowedCcdPenetration(btScalar(0.04)),
     49                m_useConvexConservativeDistanceUtil(true),
     50                m_convexConservativeDistanceThreshold(0.0f),
    4951                m_stackAllocator(0)
    5052        {
    … …  
    5254        }
    5355        btScalar        m_timeStep;
    54         int             m_stepCount;
    55         int             m_dispatchFunc;
     56        int                     m_stepCount;
     57        int                     m_dispatchFunc;
    5658        mutable btScalar        m_timeOfImpact;
    57         bool    m_useContinuous;
     59        bool            m_useContinuous;
    5860        class btIDebugDraw*     m_debugDraw;
    59         bool    m_enableSatConvex;
    60         bool    m_enableSPU;
    61         bool    m_useEpa;
     61        bool            m_enableSatConvex;
     62        bool            m_enableSPU;
     63        bool            m_useEpa;
    6264        btScalar        m_allowedCcdPenetration;
     65        bool            m_useConvexConservativeDistanceUtil;
     66        btScalar        m_convexConservativeDistanceThreshold;
    6367        btStackAlloc*   m_stackAllocator;
    64        
    6568};
    6669

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

    r2192 r2430  
    150150
    151151
     152void    btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
     153{
     154        btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
     155        aabbMin = multiProxy->m_aabbMin;
     156        aabbMax = multiProxy->m_aabbMax;
     157}
     158
     159void    btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
     160{
     161        for (int i=0;i<m_multiSapProxies.size();i++)
     162        {
     163                rayCallback.process(m_multiSapProxies[i]);
     164        }
     165}
     166
     167
    152168//#include <stdio.h>
    153169
    … …  
    209225
    210226       
    211         m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
     227        if (m_optimizedAabbTree)
     228                m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
     229
    212230        int i;
    213231

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

    r2192 r2430  
    2727typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
    2828
     29///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead.
    2930///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases.
    3031///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time.
    … …  
    7374*/
    7475                btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
    75                         :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
     76                        :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
    7677                        m_aabbMin(aabbMin),
    7778                        m_aabbMax(aabbMax),
    … …  
    109110        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    110111        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
     112        virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
     113
     114        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));
    111115
    112116        void    addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*        childBroadphase);

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

    r2192 r2430  
    3434btHashedOverlappingPairCache::btHashedOverlappingPairCache():
    3535        m_overlapFilterCallback(0),
    36         m_blockedForChanges(false)
     36        m_blockedForChanges(false),
     37        m_ghostPairCallback(0)
    3738{
    3839        int initialAllocatedSize= 2;
    … …  
    4647btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
    4748{
    48         //todo/test: show we erase/delete data, or is it automatic
    4949}
    5050
    … …  
    239239        int oldCapacity = m_overlappingPairArray.capacity();
    240240        void* mem = &m_overlappingPairArray.expand();
     241
     242        //this is where we add an actual pair, so also call the 'ghost'
     243        if (m_ghostPairCallback)
     244                m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
     245
    241246        int newCapacity = m_overlappingPairArray.capacity();
    242247
    … …  
    252257//      pair->m_pProxy1 = proxy1;
    253258        pair->m_algorithm = 0;
    254         pair->m_userInfo = 0;
     259        pair->m_internalTmpValue = 0;
    255260       
    256261
    … …  
    283288        cleanOverlappingPair(*pair,dispatcher);
    284289
    285         void* userData = pair->m_userInfo;
     290        void* userData = pair->m_internalInfo1;
    286291
    287292        btAssert(pair->m_pProxy0->getUid() == proxyId1);
    … …  
    317322
    318323        int lastPairIndex = m_overlappingPairArray.size() - 1;
     324
     325        if (m_ghostPairCallback)
     326                m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
    319327
    320328        // If the removed pair is the last pair, we are done.
    … …  
    398406                        gOverlappingPairs--;
    399407                        btBroadphasePair& pair = m_overlappingPairArray[findIndex];
    400                         void* userData = pair.m_userInfo;
     408                        void* userData = pair.m_internalInfo1;
    401409                        cleanOverlappingPair(pair,dispatcher);
     410                        if (m_ghostPairCallback)
     411                                m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
    402412                       
    403413                        m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
    … …  
    427437        void* mem = &m_overlappingPairArray.expand();
    428438        btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
     439       
    429440        gOverlappingPairs++;
    430441        gAddedPairs++;
     442       
     443        if (m_ghostPairCallback)
     444                m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
    431445        return pair;
    432446       
    … …  
    494508        m_blockedForChanges(false),
    495509        m_hasDeferredRemoval(true),
    496         m_overlapFilterCallback(0)
     510        m_overlapFilterCallback(0),
     511        m_ghostPairCallback(0)
    497512{
    498513        int initialAllocatedSize= 2;
    … …  
    502517btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
    503518{
    504         //todo/test: show we erase/delete data, or is it automatic
    505519}
    506520

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

    r2192 r2430  
    2222#include "btOverlappingPairCallback.h"
    2323
    24 #include "LinearMath/btPoint3.h"
    2524#include "LinearMath/btAlignedObjectArray.h"
    2625class btDispatcher;
    … …  
    8382
    8483        virtual bool    hasDeferredRemoval() = 0;
     84
     85        virtual void    setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
    8586
    8687};
    … …  
    254255        }
    255256
     257        virtual void    setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
     258        {
     259                m_ghostPairCallback = ghostPairCallback;
     260        }
     261
    256262public:
    257263       
    258264        btAlignedObjectArray<int>       m_hashTable;
    259265        btAlignedObjectArray<int>       m_next;
     266        btOverlappingPairCallback*      m_ghostPairCallback;
    260267       
    261268};
    … …  
    280287                //if set, use the callback instead of the built in filter in needBroadphaseCollision
    281288                btOverlapFilterCallback* m_overlapFilterCallback;
     289
     290                btOverlappingPairCallback*      m_ghostPairCallback;
    282291
    283292        public:
    … …  
    356365                }
    357366
    358 
    359 };
    360 
    361 
    362 
    363 ///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and testing.
     367                virtual void    setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
     368                {
     369                        m_ghostPairCallback = ghostPairCallback;
     370                }
     371
     372
     373};
     374
     375
     376
     377///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
    364378class btNullPairCache : public btOverlappingPairCache
    365379{
    … …  
    415429        }
    416430
     431        virtual void    setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
     432        {
     433
     434        }
     435
    417436        virtual btBroadphasePair*       addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/)
    418437        {
    … …  
    428447        {
    429448        }
     449       
    430450
    431451

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

    r2192 r2430  
    1919#include "LinearMath/btIDebugDraw.h"
    2020
    21 
    22 btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false),
     21#define RAYAABB2
     22
     23btQuantizedBvh::btQuantizedBvh() :
     24                                        m_bulletVersion(BT_BULLET_VERSION),
     25                                        m_useQuantization(false),
    2326                                        //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
    2427                                        m_traversalMode(TRAVERSAL_STACKLESS)
    2528                                        //m_traversalMode(TRAVERSAL_RECURSIVE)
    2629                                        ,m_subtreeHeaderCount(0) //PCK: add this line
    27 {
    28 
     30{
     31        m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY);
     32        m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
    2933}
    3034
    … …  
    120124        int curIndex = m_curNodeIndex;
    121125
    122         assert(numIndices>0);
     126        btAssert(numIndices>0);
    123127
    124128        if (numIndices==1)
    … …  
    141145        int internalNodeIndex = m_curNodeIndex;
    142146       
    143         setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
    144         setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
     147        //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value.
     148        //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values
     149        setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
     150        setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
     151       
    145152       
    146153        for (i=startIndex;i<endIndex;i++)
    … …  
    178185                        updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
    179186                }
     187        } else
     188        {
     189
    180190        }
    181191
    … …  
    339349int maxIterations = 0;
    340350
     351
    341352void    btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
    342353{
    … …  
    353364        {
    354365                //catch bugs in tree data
    355                 assert (walkIterations < m_curNodeIndex);
     366                btAssert (walkIterations < m_curNodeIndex);
    356367
    357368                walkIterations++;
    … …  
    435446
    436447
     448void    btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
     449{
     450        btAssert(!m_useQuantization);
     451
     452        const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
     453        int escapeIndex, curIndex = 0;
     454        int walkIterations = 0;
     455        bool isLeafNode;
     456        //PCK: unsigned instead of bool
     457        unsigned aabbOverlap=0;
     458        unsigned rayBoxOverlap=0;
     459        btScalar lambda_max = 1.0;
     460       
     461                /* Quick pruning by quantized box */
     462        btVector3 rayAabbMin = raySource;
     463        btVector3 rayAabbMax = raySource;
     464        rayAabbMin.setMin(rayTarget);
     465        rayAabbMax.setMax(rayTarget);
     466
     467        /* Add box cast extents to bounding box */
     468        rayAabbMin += aabbMin;
     469        rayAabbMax += aabbMax;
     470
     471#ifdef RAYAABB2
     472        btVector3 rayFrom = raySource;
     473        btVector3 rayDir = (rayTarget-raySource);
     474        rayDir.normalize ();
     475        lambda_max = rayDir.dot(rayTarget-raySource);
     476        ///what about division by zero? --> just set rayDirection[i] to 1.0
     477        btVector3 rayDirectionInverse;
     478        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
     479        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
     480        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
     481        unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
     482#endif
     483
     484        btVector3 bounds[2];
     485
     486        while (curIndex < m_curNodeIndex)
     487        {
     488                btScalar param = 1.0;
     489                //catch bugs in tree data
     490                btAssert (walkIterations < m_curNodeIndex);
     491
     492                walkIterations++;
     493
     494                bounds[0] = rootNode->m_aabbMinOrg;
     495                bounds[1] = rootNode->m_aabbMaxOrg;
     496                /* Add box cast extents */
     497                bounds[0] += aabbMin;
     498                bounds[1] += aabbMax;
     499
     500                aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
     501                //perhaps profile if it is worth doing the aabbOverlap test first
     502
     503#ifdef RAYAABB2
     504                        ///careful with this check: need to check division by zero (above) and fix the unQuantize method
     505                        ///thanks Joerg/hiker for the reproduction case!
     506                        ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
     507                rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false;
     508
     509#else
     510                btVector3 normal;
     511                rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal);
     512#endif
     513
     514                isLeafNode = rootNode->m_escapeIndex == -1;
     515               
     516                //PCK: unsigned instead of bool
     517                if (isLeafNode && (rayBoxOverlap != 0))
     518                {
     519                        nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
     520                }
     521               
     522                //PCK: unsigned instead of bool
     523                if ((rayBoxOverlap != 0) || isLeafNode)
     524                {
     525                        rootNode++;
     526                        curIndex++;
     527                } else
     528                {
     529                        escapeIndex = rootNode->m_escapeIndex;
     530                        rootNode += escapeIndex;
     531                        curIndex += escapeIndex;
     532                }
     533        }
     534        if (maxIterations < walkIterations)
     535                maxIterations = walkIterations;
     536
     537}
     538
    437539
    438540
    … …  
    455557
    456558        btScalar lambda_max = 1.0;
    457 #define RAYAABB2
     559
    458560#ifdef RAYAABB2
    459561        btVector3 rayFrom = raySource;
    … …  
    503605
    504606                //catch bugs in tree data
    505                 assert (walkIterations < subTreeSize);
     607                btAssert (walkIterations < subTreeSize);
    506608
    507609                walkIterations++;
    … …  
    534636                        ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
    535637
     638                        //BT_PROFILE("btRayAabb2");
    536639                        rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
     640                       
    537641#else
    538642                        rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
    … …  
    598702
    599703                //catch bugs in tree data
    600                 assert (walkIterations < subTreeSize);
     704                btAssert (walkIterations < subTreeSize);
    601705
    602706                walkIterations++;
    … …  
    653757void    btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
    654758{
    655         bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
    656         if (fast_path)
    657         {
    658                 walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex);
    659         } else {
    660                 /* Otherwise fallback to AABB overlap test */
    661                 btVector3 aabbMin = raySource;
    662                 btVector3 aabbMax = raySource;
    663                 aabbMin.setMin(rayTarget);
    664                 aabbMax.setMax(rayTarget);
    665                 reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax);
    666         }
     759        reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
    667760}
    668761
    … …  
    670763void    btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
    671764{
    672         bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
    673         if (fast_path)
     765        //always use stackless
     766
     767        if (m_useQuantization)
    674768        {
    675769                walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
    676         } else {
    677                 /* Slow path:
    678                    Construct the bounding box for the entire box cast and send that down the tree */
     770        }
     771        else
     772        {
     773                walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
     774        }
     775        /*
     776        {
     777                //recursive traversal
    679778                btVector3 qaabbMin = raySource;
    680779                btVector3 qaabbMax = raySource;
    … …  
    685784                reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
    686785        }
     786        */
     787
    687788}
    688789
    … …  
    744845bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
    745846{
    746         assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
     847        btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
    747848        m_subtreeHeaderCount = m_SubtreeHeaders.size();
    748849
    … …  
    10381139m_bvhAabbMin(self.m_bvhAabbMin),
    10391140m_bvhAabbMax(self.m_bvhAabbMax),
    1040 m_bvhQuantization(self.m_bvhQuantization)
    1041 {
    1042 
    1043 
    1044 }
    1045 
    1046 
    1047 
     1141m_bvhQuantization(self.m_bvhQuantization),
     1142m_bulletVersion(BT_BULLET_VERSION)
     1143{
     1144
     1145}
     1146
     1147
     1148

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h

    r2192 r2430  
    159159ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
    160160{
    161 protected:
    162 
    163         NodeArray                       m_leafNodes;
    164         NodeArray                       m_contiguousNodes;
    165 
    166         QuantizedNodeArray      m_quantizedLeafNodes;
    167        
    168         QuantizedNodeArray      m_quantizedContiguousNodes;
    169        
    170         int                                     m_curNodeIndex;
    171 
    172 
    173         //quantization data
    174         bool                            m_useQuantization;
    175         btVector3                       m_bvhAabbMin;
    176         btVector3                       m_bvhAabbMax;
    177         btVector3                       m_bvhQuantization;
    178161public:
    179         BT_DECLARE_ALIGNED_ALLOCATOR();
    180 
    181162        enum btTraversalMode
    182163        {
    … …  
    185166                TRAVERSAL_RECURSIVE
    186167        };
     168
    187169protected:
    188170
     171
     172        btVector3                       m_bvhAabbMin;
     173        btVector3                       m_bvhAabbMax;
     174        btVector3                       m_bvhQuantization;
     175
     176        int                                     m_bulletVersion;        //for serialization versioning. It could also be used to detect endianess.
     177
     178        int                                     m_curNodeIndex;
     179        //quantization data
     180        bool                            m_useQuantization;
     181
     182
     183
     184        NodeArray                       m_leafNodes;
     185        NodeArray                       m_contiguousNodes;
     186        QuantizedNodeArray      m_quantizedLeafNodes;
     187        QuantizedNodeArray      m_quantizedContiguousNodes;
     188       
    189189        btTraversalMode m_traversalMode;
    190        
    191190        BvhSubtreeInfoArray             m_SubtreeHeaders;
    192191
    193192        //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
    194193        int m_subtreeHeaderCount;
     194
     195       
     196
    195197
    196198
    … …  
    297299        void    walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
    298300        void    walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
     301        void    walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
    299302
    300303        ///tree traversal designed for small-memory processors like PS3 SPU
    … …  
    308311       
    309312
    310 #define USE_BANCHLESS 1
    311 #ifdef USE_BANCHLESS
    312         //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
    313         SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
    314         {               
    315                 return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
    316                         & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
    317                         & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
    318                         1, 0));
    319         }
    320 #else
    321         SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
    322         {
    323                 bool overlap = true;
    324                 overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
    325                 overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
    326                 overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
    327                 return overlap;
    328         }
    329 #endif //USE_BANCHLESS
     313
    330314
    331315        void    updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
    332316
    333317public:
     318       
     319        BT_DECLARE_ALIGNED_ALLOCATOR();
     320
    334321        btQuantizedBvh();
    335322
    … …  
    364351                ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
    365352                ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
    366                 ///todo: double-check this
     353                ///@todo: double-check this
    367354                if (isMax)
    368355                {

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

    r2192 r2430  
    5656        m_numHandles = 0;
    5757        m_firstFreeHandle = 0;
     58        m_LastHandleIndex = -1;
    5859       
    5960
    … …  
    8990                return 0; //should never happen, but don't let the game crash ;-)
    9091        }
    91         assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
     92        btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
    9293
    9394        int newHandleIndex = allocHandle();
    … …  
    138139}
    139140
     141void    btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
     142{
     143        const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
     144        aabbMin = sbp->m_aabbMin;
     145        aabbMax = sbp->m_aabbMax;
     146}
     147
    140148void    btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
    141149{
    142150        btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
    143         sbp->m_min = aabbMin;
    144         sbp->m_max = aabbMax;
    145 }
    146 
    147 
     151        sbp->m_aabbMin = aabbMin;
     152        sbp->m_aabbMax = aabbMax;
     153}
     154
     155void    btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
     156{
     157        for (int i=0; i <= m_LastHandleIndex; i++)
     158        {
     159                btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
     160                if(!proxy->m_clientObject)
     161                {
     162                        continue;
     163                }
     164                rayCallback.process(proxy);
     165        }
     166}
    148167
    149168
    … …  
    155174bool    btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
    156175{
    157         return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] &&
    158                    proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] &&
    159                    proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2];
     176        return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
     177                   proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
     178                   proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
    160179
    161180}
    … …  
    177196        //first check for new overlapping pairs
    178197        int i,j;
    179 
    180198        if (m_numHandles >= 0)
    181199        {
    182 
    183                 for (i=0;i<m_numHandles;i++)
     200                int new_largest_index = -1;
     201                for (i=0; i <= m_LastHandleIndex; i++)
    184202                {
    185203                        btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
    186 
    187                         for (j=i+1;j<m_numHandles;j++)
     204                        if(!proxy0->m_clientObject)
     205                        {
     206                                continue;
     207                        }
     208                        new_largest_index = i;
     209                        for (j=i+1; j <= m_LastHandleIndex; j++)
    188210                        {
    189211                                btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
    190212                                btAssert(proxy0 != proxy1);
     213                                if(!proxy1->m_clientObject)
     214                                {
     215                                        continue;
     216                                }
    191217
    192218                                btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
    … …  
    212238                }
    213239
     240                m_LastHandleIndex = new_largest_index;
     241
    214242                if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
    215243                {

  • code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

    r2192 r2430  
    2323struct btSimpleBroadphaseProxy : public btBroadphaseProxy
    2424{
    25         btVector3       m_min;
    26         btVector3       m_max;
    2725        int                     m_nextFree;
    2826       
    … …  
    3230        btSimpleBroadphaseProxy() {};
    3331
    34         btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
    35         :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy),
    36         m_min(minpt),m_max(maxpt)               
     32        btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
     33        :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
    3734        {
    3835                (void)shapeType;
    … …  
    5754        int             m_numHandles;                                           // number of active handles
    5855        int             m_maxHandles;                                           // max number of handles
     56        int             m_LastHandleIndex;                                                     
    5957       
    6058        btSimpleBroadphaseProxy* m_pHandles;                                            // handles pool
    … …  
    6967                m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
    7068                m_numHandles++;
     69                if(freeHandle > m_LastHandleIndex)
     70                {
     71                        m_LastHandleIndex = freeHandle;
     72                }
    7173                return freeHandle;
    7274        }
    … …  
    7678                int handle = int(proxy-m_pHandles);
    7779                btAssert(handle >= 0 && handle < m_maxHandles);
    78 
     80                if(handle == m_LastHandleIndex)
     81                {
     82                        m_LastHandleIndex--;
     83                }
    7984                proxy->SetNextFree(m_firstFreeHandle);
    8085                m_firstFreeHandle = handle;
     86
     87                proxy->m_clientObject = 0;
    8188
    8289                m_numHandles--;
    … …  
    93100        {
    94101                btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
     102                return proxy0;
     103        }
     104
     105        inline const btSimpleBroadphaseProxy*   getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
     106        {
     107                const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
    95108                return proxy0;
    96109        }
    … …  
    118131        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    119132        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
     133        virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
     134
     135        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));
    120136               
    121137        btOverlappingPairCache* getOverlappingPairCache()
Note: See TracChangeset for help on using the changeset viewer.