Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Oct 20, 2008, 5:40:38 PM (16 years ago)
Author:
rgrieder
Message:

Downgraded Bullet to latest tagged version: 2.72
That should give us more stability.

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

Legend:

Unmodified
Added
Removed

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

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

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

    r1963 r1972  
    2626#include "btBroadphaseProxy.h"
    2727#include "btOverlappingPairCallback.h"
    28 #include "btDbvtBroadphase.h"
    2928
    3029//#define DEBUG_BROADPHASE 1
    … …  
    6362                BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];            // 6 * 2 = 12
    6463//              BP_FP_INT_TYPE m_uniqueId;
    65                 btBroadphaseProxy*      m_dbvtProxy;//for faster raycast
     64                BP_FP_INT_TYPE m_pad;
     65               
    6666                //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
    6767       
    … …  
    9595        int     m_invalidPair;
    9696
    97         ///additional dynamic aabb structure, used to accelerate ray cast queries.
    98         ///can be disabled using a optional argument in the constructor
    99         btDbvtBroadphase*       m_raycastAccelerator;
    100 
    101 
    10297        // allocation/deallocation
    10398        BP_FP_INT_TYPE allocHandle();
    … …  
    114109        //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
    115110
    116        
     111        void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
    117112
    118113        void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
    … …  
    123118public:
    124119
    125         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,bool disableRaycastAccelerator = false);
     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);
    126121
    127122        virtual ~btAxisSweep3Internal();
    … …  
    145140        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    146141        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
    147         virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
    148        
    149         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
    150 
    151         void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
    152         ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
    153         void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
    154142       
    155143        bool    testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
    … …  
    230218               
    231219                Handle* handle = getHandle(handleId);
    232                
    233                 if (m_raycastAccelerator)
    234                 {
    235                         btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
    236                         handle->m_dbvtProxy = rayProxy;
    237                 }
     220                               
    238221                return handle;
    239222}
    … …  
    245228{
    246229        Handle* handle = static_cast<Handle*>(proxy);
    247         if (m_raycastAccelerator)
    248                 m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
    249230        removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
    250231}
    … …  
    254235{
    255236        Handle* handle = static_cast<Handle*>(proxy);
    256         handle->m_aabbMin = aabbMin;
    257         handle->m_aabbMax = aabbMax;
    258237        updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
    259         if (m_raycastAccelerator)
    260                 m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
    261 
    262 }
    263 
    264 template <typename BP_FP_INT_TYPE>
    265 
    266 void    btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
    267 {
    268         if (m_raycastAccelerator)
    269         {
    270                 m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback);
    271         } else
    272         {
    273                 //choose axis?
    274                 BP_FP_INT_TYPE axis = 0;
    275                 //for each proxy
    276                 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
    277                 {
    278                         if (m_pEdges[axis][i].IsMax())
    279                         {
    280                                 rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
    281                         }
    282                 }
    283         }
    284 }
    285 
    286 
    287 template <typename BP_FP_INT_TYPE>
    288 void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
    289 {
    290         Handle* pHandle = static_cast<Handle*>(proxy);
    291         aabbMin = pHandle->m_aabbMin;
    292         aabbMax = pHandle->m_aabbMax;
    293 }
    294 
    295 
    296 template <typename BP_FP_INT_TYPE>
    297 void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
    298 {
    299         Handle* pHandle = static_cast<Handle*>(proxy);
    300 
    301         unsigned short vecInMin[3];
    302         unsigned short vecInMax[3];
    303 
    304         vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
    305         vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
    306         vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
    307         vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
    308         vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
    309         vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
    310        
    311         aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
    312         aabbMin += m_worldAabbMin;
    313        
    314         aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
    315         aabbMax += m_worldAabbMin;
    316 }
    317 
    318 
    319 
    320 
    321 template <typename BP_FP_INT_TYPE>
    322 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 , bool disableRaycastAccelerator)
     238
     239}
     240
     241
     242
     243
     244
     245template <typename BP_FP_INT_TYPE>
     246btAxisSweep3Internal<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 )
    323247:m_bpHandleMask(handleMask),
    324248m_handleSentinel(handleSentinel),
    … …  
    326250m_userPairCallback(0),
    327251m_ownsPairCache(false),
    328 m_invalidPair(0),
    329 m_raycastAccelerator(0)
     252m_invalidPair(0)
    330253{
    331254        BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
    … …  
    336259                m_pairCache = new(ptr) btHashedOverlappingPairCache();
    337260                m_ownsPairCache = true;
    338         }
    339 
    340         if (!disableRaycastAccelerator)
    341         {
    342                 m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase();//m_pairCache);
    343                 m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
    344261        }
    345262
    … …  
    404321btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
    405322{
    406         if (m_raycastAccelerator)
    407                 btAlignedFree (m_raycastAccelerator);
    408 
     323       
    409324        for (int i = 2; i >= 0; i--)
    410325        {
    … …  
    985900public:
    986901
    987         btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
     902        btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
    988903
    989904};
    … …  
    996911public:
    997912
    998         bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
     913        bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
    999914
    1000915};

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

    r1963 r1972  
    2424class btOverlappingPairCache;
    2525
    26 struct  btBroadphaseRayCallback
    27 {
    28         virtual ~btBroadphaseRayCallback() {}
    29         virtual bool    process(const btBroadphaseProxy* proxy) = 0;
    30 };
    31 
    3226#include "LinearMath/btVector3.h"
    3327
    … …  
    4337        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
    4438        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
    45         virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
    46 
    47         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback) = 0;
    48 
     39       
    4940        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
    5041        virtual void    calculateOverlappingPairs(btDispatcher* dispatcher)=0;

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

    r1963 r1972  
    1818
    1919#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
    20 #include "LinearMath/btVector3.h"
    2120#include "LinearMath/btAlignedAllocator.h"
    2221
    … …  
    9291        };
    9392
    94         btVector3       m_aabbMin;
    95         btVector3       m_aabbMax;
    96 
    9793        //Usually the client btCollisionObject or Rigidbody class
    9894        void*   m_clientObject;
    … …  
    116112        }
    117113
    118         btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
    119                 :m_aabbMin(aabbMin),
    120                 m_aabbMax(aabbMax),
    121                 m_clientObject(userPtr),
     114        btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
     115                :m_clientObject(userPtr),
    122116                m_collisionFilterGroup(collisionFilterGroup),
    123117                m_collisionFilterMask(collisionFilterMask)

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

    r1963 r1972  
    2424struct btDbvtNodeEnumerator : btDbvt::ICollide
    2525{
    26         tConstNodeArray nodes;
    27         void Process(const btDbvtNode* n) { nodes.push_back(n); }
     26tConstNodeArray nodes;
     27void 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);
     33return(node->parent->childs[1]==node);
    3434}
    3535
    3636//
    3737static DBVT_INLINE btDbvtVolume merge(  const btDbvtVolume& a,
    38                                                                           const btDbvtVolume& b)
     38                                                                                const btDbvtVolume& b)
    3939{
    4040#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
    41         DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)];
    42         btDbvtVolume&   res=*(btDbvtVolume*)locals;
     41DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)];
     42btDbvtVolume&   res=*(btDbvtVolume*)locals;
    4343#else
    44         btDbvtVolume    res;
     44btDbvtVolume    res;
    4545#endif
    46         Merge(a,b,res);
    47         return(res);
     46Merge(a,b,res);
     47return(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()+
     53const btVector3 edges=a.Lengths();
     54return( 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);
     61if(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{
     72btAlignedFree(pdbvt->m_free);
     73pdbvt->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{
     80if(!node->isleaf())
     81        {
     82        recursedeletenode(pdbvt,node->childs[0]);
     83        recursedeletenode(pdbvt,node->childs[1]);
     84        }
     85if(node==pdbvt->m_root) pdbvt->m_root=0;
     86deletenode(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{
     94btDbvtNode*     node;
     95if(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);
     99node->parent    =       parent;
     100node->data              =       data;
     101node->childs[1] =       0;
     102return(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{
     111btDbvtNode*     node=createnode(pdbvt,parent,data);
     112node->volume=volume;
     113return(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{
     123btDbvtNode*     node=createnode(pdbvt,parent,data);
     124Merge(volume0,volume1,node->volume);
     125return(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{
     133if(!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))
    157                                         Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->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);
    158158                                else
    159                                         break;
    160                                 node=prev;
     159                                break;
     160                        node=prev;
    161161                        } while(0!=(prev=node->parent));
    162162                }
    163163                else
    164164                {
    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 
     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       
    172172//
    173173static btDbvtNode*                              removeleaf(     btDbvt* pdbvt,
    174                                                                                    btDbvtNode* leaf)
    175 {
    176         if(leaf==pdbvt->m_root)
    177         {
    178                 pdbvt->m_root=0;
    179                 return(0);
     174                                                                                        btDbvtNode* leaf)
     175{
     176if(leaf==pdbvt->m_root)
     177        {
     178        pdbvt->m_root=0;
     179        return(0);
    180180        }
    181181        else
    182182        {
    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))
     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))
    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);
     218if(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{
     237left.resize(0);
     238right.resize(0);
     239for(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;
     252DBVT_ALIGN char locals[sizeof(btDbvtVolume)];
     253btDbvtVolume&   volume=*(btDbvtVolume*)locals;
     254volume=leaves[0]->volume;
    255255#else
    256         btDbvtVolume volume=leaves[0]->volume;
     256btDbvtVolume 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);
     258for(int i=1,ni=leaves.size();i<ni;++i)
     259        {
     260        Merge(volume,leaves[i]->volume,volume);
     261        }
     262return(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{
     269while(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)
     303static const btVector3  axis[]={btVector3(1,0,0),
     304                                                                btVector3(0,1,0),
     305                                                                btVector3(0,0,1)};
     306if(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        }
     365return(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);
     371btDbvtNode*     p=n->parent;
     372btAssert(n->isinternal());
     373if(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        }
     393return(n);
    394394}
    395395
    … …  
    397397static DBVT_INLINE btDbvtNode*  walkup(btDbvtNode* n,int count)
    398398{
    399         while(n&&(count--)) n=n->parent;
    400         return(n);
     399while(n&&(count--)) n=n->parent;
     400return(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();
     408                                btDbvt::btDbvt()
     409{
     410m_root          =       0;
     411m_free          =       0;
     412m_lkhd          =       -1;
     413m_leaves        =       0;
     414m_opath         =       0;
     415}
     416
     417//
     418                                btDbvt::~btDbvt()
     419{
     420clear();
    421421}
    422422
    … …  
    424424void                    btDbvt::clear()
    425425{
    426         if(m_root)      recursedeletenode(this,m_root);
    427         btAlignedFree(m_free);
    428         m_free=0;
     426if(m_root)      recursedeletenode(this,m_root);
     427btAlignedFree(m_free);
     428m_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];
     434if(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);
     447if(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;
     459if(passes<0) passes=m_leaves;
     460if(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);
     479btDbvtNode*     leaf=createnode(this,0,volume,data);
     480insertleaf(this,m_root,leaf);
     481++m_leaves;
     482return(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;
     488btDbvtNode*     root=removeleaf(this,leaf);
     489if(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);
     499insertleaf(this,root,leaf);
    500500}
    501501
    … …  
    503503void                    btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume)
    504504{
    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;
     505btDbvtNode*     root=removeleaf(this,leaf);
     506if(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);
     516leaf->volume=volume;
     517insertleaf(this,root,leaf);
    518518}
    519519
    … …  
    521521bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin)
    522522{
    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);
     523if(leaf->volume.Contain(volume)) return(false);
     524volume.Expand(btVector3(margin,margin,margin));
     525volume.SignedExpand(velocity);
     526update(leaf,volume);
     527return(true);
    528528}
    529529
    … …  
    531531bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity)
    532532{
    533         if(leaf->volume.Contain(volume)) return(false);
    534         volume.SignedExpand(velocity);
    535         update(leaf,volume);
    536         return(true);
     533if(leaf->volume.Contain(volume)) return(false);
     534volume.SignedExpand(velocity);
     535update(leaf,volume);
     536return(true);
    537537}
    538538
    … …  
    540540bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin)
    541541{
    542         if(leaf->volume.Contain(volume)) return(false);
    543         volume.Expand(btVector3(margin,margin,margin));
    544         update(leaf,volume);
    545         return(true);
     542if(leaf->volume.Contain(volume)) return(false);
     543volume.Expand(btVector3(margin,margin,margin));
     544update(leaf,volume);
     545return(true);
    546546}
    547547
    … …  
    549549void                    btDbvt::remove(btDbvtNode* leaf)
    550550{
    551         removeleaf(this,leaf);
    552         deletenode(this,leaf);
    553         --m_leaves;
     551removeleaf(this,leaf);
     552deletenode(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);
     559btDbvtNodeEnumerator    nodes;
     560nodes.nodes.reserve(m_leaves*2);
     561enumNodes(m_root,nodes);
     562iwriter->Prepare(m_root,nodes.nodes.size());
     563for(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)
     584dest.clear();
     585if(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);
     615int     depth=0;
     616if(node) getmaxdepth(node,1,depth);
     617return(depth);
    618618}
    619619
    … …  
    621621int                             btDbvt::countLeaves(const btDbvtNode* node)
    622622{
    623         if(node->isinternal())
    624                 return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
     623if(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);
     632if(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
     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
    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::rayTest: 6314 ms (0%),(332143 r/s)
     672[7] btDbvt::collideRAY: 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)
     687struct 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;
     702struct 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;
     723struct 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);
     745static btScalar                 RandUnit()
     746        {
     747        return(rand()/(btScalar)RAND_MAX);
     748        }
     749static btVector3                RandVector3()
     750        {
     751        return(btVector3(RandUnit(),RandUnit(),RandUnit()));
     752        }
     753static btVector3                RandVector3(btScalar cs)
     754        {
     755        return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
     756        }
     757static btDbvtVolume     RandVolume(btScalar cs,btScalar eb,btScalar es)
     758        {
     759        return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
     760        }
     761static 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        }
     768static 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::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)
     780static const btScalar   cfgVolumeCenterScale            =       100;
     781static const btScalar   cfgVolumeExentsBase                     =       1;
     782static const btScalar   cfgVolumeExentsScale            =       4;
     783static const int                cfgLeaves                                       =       8192;
     784static const bool               cfgEnable                                       =       true;
     785
     786//[1] btDbvtVolume intersections
     787bool                                    cfgBenchmark1_Enable            =       cfgEnable;
     788static const int                cfgBenchmark1_Iterations        =       8;
     789static const int                cfgBenchmark1_Reference         =       3499;
     790//[2] btDbvtVolume merges
     791bool                                    cfgBenchmark2_Enable            =       cfgEnable;
     792static const int                cfgBenchmark2_Iterations        =       4;
     793static const int                cfgBenchmark2_Reference         =       1945;
     794//[3] btDbvt::collideTT
     795bool                                    cfgBenchmark3_Enable            =       cfgEnable;
     796static const int                cfgBenchmark3_Iterations        =       512;
     797static const int                cfgBenchmark3_Reference         =       5485;
     798//[4] btDbvt::collideTT self
     799bool                                    cfgBenchmark4_Enable            =       cfgEnable;
     800static const int                cfgBenchmark4_Iterations        =       512;
     801static const int                cfgBenchmark4_Reference         =       2814;
     802//[5] btDbvt::collideTT xform
     803bool                                    cfgBenchmark5_Enable            =       cfgEnable;
     804static const int                cfgBenchmark5_Iterations        =       512;
     805static const btScalar   cfgBenchmark5_OffsetScale       =       2;
     806static const int                cfgBenchmark5_Reference         =       7379;
     807//[6] btDbvt::collideTT xform,self
     808bool                                    cfgBenchmark6_Enable            =       cfgEnable;
     809static const int                cfgBenchmark6_Iterations        =       512;
     810static const btScalar   cfgBenchmark6_OffsetScale       =       2;
     811static const int                cfgBenchmark6_Reference         =       7270;
     812//[7] btDbvt::collideRAY
     813bool                                    cfgBenchmark7_Enable            =       cfgEnable;
     814static const int                cfgBenchmark7_Passes            =       32;
     815static const int                cfgBenchmark7_Iterations        =       65536;
     816static const int                cfgBenchmark7_Reference         =       6307;
     817//[8] insert/remove
     818bool                                    cfgBenchmark8_Enable            =       cfgEnable;
     819static const int                cfgBenchmark8_Passes            =       32;
     820static const int                cfgBenchmark8_Iterations        =       65536;
     821static const int                cfgBenchmark8_Reference         =       2105;
     822//[9] updates (teleport)
     823bool                                    cfgBenchmark9_Enable            =       cfgEnable;
     824static const int                cfgBenchmark9_Passes            =       32;
     825static const int                cfgBenchmark9_Iterations        =       65536;
     826static const int                cfgBenchmark9_Reference         =       1879;
     827//[10] updates (jitter)
     828bool                                    cfgBenchmark10_Enable           =       cfgEnable;
     829static const btScalar   cfgBenchmark10_Scale            =       cfgVolumeCenterScale/10000;
     830static const int                cfgBenchmark10_Passes           =       32;
     831static const int                cfgBenchmark10_Iterations       =       65536;
     832static const int                cfgBenchmark10_Reference        =       1244;
     833//[11] optimize (incremental)
     834bool                                    cfgBenchmark11_Enable           =       cfgEnable;
     835static const int                cfgBenchmark11_Passes           =       64;
     836static const int                cfgBenchmark11_Iterations       =       65536;
     837static const int                cfgBenchmark11_Reference        =       2510;
     838//[12] btDbvtVolume notequal
     839bool                                    cfgBenchmark12_Enable           =       cfgEnable;
     840static const int                cfgBenchmark12_Iterations       =       32;
     841static const int                cfgBenchmark12_Reference        =       3677;
     842//[13] culling(OCL+fullsort)
     843bool                                    cfgBenchmark13_Enable           =       cfgEnable;
     844static const int                cfgBenchmark13_Iterations       =       1024;
     845static const int                cfgBenchmark13_Reference        =       2231;
     846//[14] culling(OCL+qsort)
     847bool                                    cfgBenchmark14_Enable           =       cfgEnable;
     848static const int                cfgBenchmark14_Iterations       =       8192;
     849static const int                cfgBenchmark14_Reference        =       3500;
     850//[15] culling(KDOP+qsort)
     851bool                                    cfgBenchmark15_Enable           =       cfgEnable;
     852static const int                cfgBenchmark15_Iterations       =       8192;
     853static const int                cfgBenchmark15_Reference        =       1151;
     854//[16] insert/remove batch
     855bool                                    cfgBenchmark16_Enable           =       cfgEnable;
     856static const int                cfgBenchmark16_BatchCount       =       256;
     857static const int                cfgBenchmark16_Passes           =       16384;
     858static const int                cfgBenchmark16_Reference        =       5138;
     859//[17] select
     860bool                                    cfgBenchmark17_Enable           =       cfgEnable;
     861static const int                cfgBenchmark17_Iterations       =       4;
     862static const int                cfgBenchmark17_Reference        =       3390;
     863
     864btClock                                 wallclock;
     865printf("Benchmarking dbvt...\r\n");
     866printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
     867printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
     868printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
     869printf("\tLeaves: %u\r\n",cfgLeaves);
     870printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
     871printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
     872if(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        }
     898if(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        }
     924if(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        }
     942if(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        }
     958if(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        }
     982if(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        }
     1004if(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::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)
     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        }
     1033if(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        }
     1052if(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        }
     1074if(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        }
     1105if(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        }
     1121if(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        }
     1147if(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        }
     1172if(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        }
     1199if(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        }
     1227if(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        }
     1253if(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        }
     1287printf("\r\n\r\n");
    12881288}
    12891289#endif

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

    r1963 r1972  
    2121#include "LinearMath/btVector3.h"
    2222#include "LinearMath/btTransform.h"
    23 #include "LinearMath/btAabbUtil2.h"
    2423
    2524//
    … …  
    3433// Template implementation of ICollide
    3534#ifdef WIN32
    36 #if (defined (_MSC_VER) && _MSC_VER >= 1400)
    37 #define DBVT_USE_TEMPLATE               1
    38 #else
    39 #define DBVT_USE_TEMPLATE               0
     35        #if (defined (_MSC_VER) && _MSC_VER >= 1400)
     36        #define DBVT_USE_TEMPLATE               1
     37        #else
     38        #define DBVT_USE_TEMPLATE               0
    4039#endif
    4140#else
    … …  
    136135struct  btDbvtAabbMm
    137136{
    138         DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
    139         DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
    140         DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
    141         DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
    142         DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
    143         static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
    144         static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
    145         static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
    146         static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
    147         static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
    148         DBVT_INLINE void                                Expand(const btVector3& e);
    149         DBVT_INLINE void                                SignedExpand(const btVector3& e);
    150         DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
    151         DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
    152         DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
    153         DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    154                 const btDbvtAabbMm& b);
    155         DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    156                 const btDbvtAabbMm& b,
    157                 const btTransform& xform);
    158         DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    159                 const btVector3& b);
    160 
    161         DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
    162                 const btDbvtAabbMm& b);
    163         DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
    164                 const btDbvtAabbMm& a,
    165                 const btDbvtAabbMm& b);
    166         DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
    167                 const btDbvtAabbMm& b,
    168                 btDbvtAabbMm& r);
    169         DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
    170                 const btDbvtAabbMm& b);
     137DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
     138DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
     139DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
     140DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
     141DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
     142static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
     143static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
     144static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
     145static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
     146static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
     147DBVT_INLINE void                                Expand(const btVector3& e);
     148DBVT_INLINE void                                SignedExpand(const btVector3& e);
     149DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
     150DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
     151DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
     152DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     153                                                                                        const btDbvtAabbMm& b);
     154DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     155                                                                                        const btDbvtAabbMm& b,
     156                                                                                        const btTransform& xform);
     157DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     158                                                                                        const btVector3& b);
     159DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     160                                                                                        const btVector3& org,
     161                                                                                        const btVector3& invdir,
     162                                                                                        const unsigned* signs);
     163DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
     164                                                                                        const btDbvtAabbMm& b);
     165DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
     166                                                                                        const btDbvtAabbMm& a,
     167                                                                                        const btDbvtAabbMm& b);
     168DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
     169                                                                                        const btDbvtAabbMm& b,
     170                                                                                        btDbvtAabbMm& r);
     171DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
     172                                                                                        const btDbvtAabbMm& b);
    171173private:
    172         DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
     174DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
    173175private:
    174         btVector3       mi,mx;
     176btVector3       mi,mx;
    175177};
    176178
    … …  
    186188        DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
    187189        union   {
    188                 btDbvtNode*     childs[2];
    189                 void*   data;
    190                 int             dataAsInt;
    191         };
     190                        btDbvtNode*     childs[2];
     191                        void*   data;
     192                        int             dataAsInt;
     193                        };
    192194};
    193195
    … …  
    196198///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
    197199struct  btDbvt
    198 {
     200        {
    199201        /* Stack element        */
    200202        struct  sStkNN
    201         {
     203                {
    202204                const btDbvtNode*       a;
    203205                const btDbvtNode*       b;
    204206                sStkNN() {}
    205207                sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
    206         };
     208                };
    207209        struct  sStkNP
    208         {
     210                {
    209211                const btDbvtNode*       node;
    210212                int                     mask;
    211213                sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
    212         };
     214                };
    213215        struct  sStkNPS
    214         {
     216                {
    215217                const btDbvtNode*       node;
    216218                int                     mask;
    … …  
    218220                sStkNPS() {}
    219221                sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
    220         };
     222                };
    221223        struct  sStkCLN
    222         {
     224                {
    223225                const btDbvtNode*       node;
    224226                btDbvtNode*             parent;
    225227                sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
    226         };
     228                };
    227229        // Policies/Interfaces
    228 
     230                       
    229231        /* ICollide     */
    230232        struct  ICollide
    231         {               
     233                {               
    232234                DBVT_VIRTUAL_DTOR(ICollide)
    233                         DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
     235                DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
    234236                DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
    235237                DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
    236238                DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
    237239                DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
    238         };
     240                };
    239241        /* IWriter      */
    240242        struct  IWriter
    241         {
     243                {
    242244                virtual ~IWriter() {}
    243245                virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
    244246                virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
    245247                virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
    246         };
     248                };
    247249        /* IClone       */
    248250        struct  IClone
    249         {
     251                {
    250252                virtual ~IClone()       {}
    251253                virtual void            CloneLeaf(btDbvtNode*) {}
    252         };
    253 
     254                };
     255               
    254256        // Constants
    255257        enum    {
    256                 SIMPLE_STACKSIZE        =       64,
    257                 DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
    258         };
    259 
     258                        SIMPLE_STACKSIZE        =       64,
     259                        DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
     260                        };
     261               
    260262        // Fields
    261263        btDbvtNode*             m_root;
    … …  
    265267        unsigned                m_opath;
    266268        // Methods
    267         btDbvt();
    268         ~btDbvt();
     269                                        btDbvt();
     270                                        ~btDbvt();
    269271        void                    clear();
    270272        bool                    empty() const { return(0==m_root); }
    … …  
    284286        static int              countLeaves(const btDbvtNode* node);
    285287        static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
    286 #if DBVT_ENABLE_BENCHMARK
     288        #if DBVT_ENABLE_BENCHMARK
    287289        static void             benchmark();
    288 #else
     290        #else
    289291        static void             benchmark(){}
    290 #endif
     292        #endif
    291293        // DBVT_IPOLICY must support ICollide policy/interface
    292294        DBVT_PREFIX
    293                 static void             enumNodes(      const btDbvtNode* root,
    294                 DBVT_IPOLICY);
     295        static void             enumNodes(      const btDbvtNode* root,
     296                                                                DBVT_IPOLICY);
    295297        DBVT_PREFIX
    296                 static void             enumLeaves(     const btDbvtNode* root,
    297                 DBVT_IPOLICY);
     298        static void             enumLeaves(     const btDbvtNode* root,
     299                                                                DBVT_IPOLICY);
    298300        DBVT_PREFIX
    299                 static void             collideTT(      const btDbvtNode* root0,
    300                 const btDbvtNode* root1,
    301                 DBVT_IPOLICY);
     301        static void             collideTT(      const btDbvtNode* root0,
     302                                                                const btDbvtNode* root1,
     303                                                                DBVT_IPOLICY);
    302304        DBVT_PREFIX
    303                 static void             collideTT(      const btDbvtNode* root0,
    304                 const btDbvtNode* root1,
    305                 const btTransform& xform,
    306                 DBVT_IPOLICY);
     305        static void             collideTT(      const btDbvtNode* root0,
     306                                                                const btDbvtNode* root1,
     307                                                                const btTransform& xform,
     308                                                                DBVT_IPOLICY);
    307309        DBVT_PREFIX
    308                 static void             collideTT(      const btDbvtNode* root0,
    309                 const btTransform& xform0,
    310                 const btDbvtNode* root1,
    311                 const btTransform& xform1,
    312                 DBVT_IPOLICY);
     310        static void             collideTT(      const btDbvtNode* root0,
     311                                                                const btTransform& xform0,
     312                                                                const btDbvtNode* root1,
     313                                                                const btTransform& xform1,
     314                                                                DBVT_IPOLICY);
    313315        DBVT_PREFIX
    314                 static void             collideTV(      const btDbvtNode* root,
    315                 const btDbvtVolume& volume,
    316                 DBVT_IPOLICY);
     316        static void             collideTV(      const btDbvtNode* root,
     317                                                                const btDbvtVolume& volume,
     318                                                                DBVT_IPOLICY);
    317319        DBVT_PREFIX
    318                 static void             rayTest(        const btDbvtNode* root,
    319                 const btVector3& rayFrom,
    320                 const btVector3& rayTo,
    321                 DBVT_IPOLICY);
     320        static void             collideRAY(     const btDbvtNode* root,
     321                                                                const btVector3& origin,
     322                                                                const btVector3& direction,
     323                                                                DBVT_IPOLICY);
    322324        DBVT_PREFIX
    323                 static void             collideKDOP(const btDbvtNode* root,
    324                 const btVector3* normals,
    325                 const btScalar* offsets,
    326                 int count,
    327                 DBVT_IPOLICY);
     325        static void             collideKDOP(const btDbvtNode* root,
     326                                                                const btVector3* normals,
     327                                                                const btScalar* offsets,
     328                                                                int count,
     329                                                                DBVT_IPOLICY);
    328330        DBVT_PREFIX
    329                 static void             collideOCL(     const btDbvtNode* root,
    330                 const btVector3* normals,
    331                 const btScalar* offsets,
    332                 const btVector3& sortaxis,
    333                 int count,                                                             
    334                 DBVT_IPOLICY,
    335                 bool fullsort=true);
     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);
    336338        DBVT_PREFIX
    337                 static void             collideTU(      const btDbvtNode* root,
    338                 DBVT_IPOLICY);
     339        static void             collideTU(      const btDbvtNode* root,
     340                                                                DBVT_IPOLICY);
    339341        // Helpers     
    340342        static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
    341         {
     343                {
    342344                int     m=0;
    343345                while(l<h)
    344                 {
     346                        {
    345347                        m=(l+h)>>1;
    346348                        if(a[i[m]].value>=v) l=m+1; else h=m;
     349                        }
     350                return(h);
    347351                }
    348                 return(h);
    349         }
    350352        static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
    351                 btAlignedObjectArray<sStkNPS>& stock,
    352                 const sStkNPS& value)
    353         {
     353                                                                                btAlignedObjectArray<sStkNPS>& stock,
     354                                                                                const sStkNPS& value)
     355                {
    354356                int     i;
    355357                if(ifree.size()>0)
    356                 { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
    357                 else
    358                 { i=stock.size();stock.push_back(value); }
     358                        { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
     359                        else
     360                        { i=stock.size();stock.push_back(value); }
    359361                return(i);
    360         }
     362                }
    361363        //
    362 private:
    363         btDbvt(const btDbvt&)   {}     
    364 };
     364        private:
     365                                        btDbvt(const btDbvt&)   {}     
     366        };
    365367
    366368//
    … …  
    371373inline btDbvtAabbMm                     btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
    372374{
    373         btDbvtAabbMm box;
    374         box.mi=c-e;box.mx=c+e;
    375         return(box);
    376 }
    377 
     375btDbvtAabbMm box;
     376box.mi=c-e;box.mx=c+e;
     377return(box);
     378}
     379       
    378380//
    379381inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
    380382{
    381         return(FromCE(c,btVector3(r,r,r)));
    382 }
    383 
     383return(FromCE(c,btVector3(r,r,r)));
     384}
     385       
    384386//
    385387inline btDbvtAabbMm                     btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
    386388{
    387         btDbvtAabbMm box;
    388         box.mi=mi;box.mx=mx;
    389         return(box);
    390 }
    391 
     389btDbvtAabbMm box;
     390box.mi=mi;box.mx=mx;
     391return(box);
     392}
     393       
    392394//
    393395inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
    394396{
    395         btDbvtAabbMm box;
    396         box.mi=box.mx=pts[0];
    397         for(int i=1;i<n;++i)
    398         {
    399                 box.mi.setMin(pts[i]);
    400                 box.mx.setMax(pts[i]);
    401         }
    402         return(box);
     397btDbvtAabbMm box;
     398box.mi=box.mx=pts[0];
     399for(int i=1;i<n;++i)
     400        {
     401        box.mi.setMin(pts[i]);
     402        box.mx.setMax(pts[i]);
     403        }
     404return(box);
    403405}
    404406
    … …  
    406408inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
    407409{
    408         btDbvtAabbMm box;
    409         box.mi=box.mx=*ppts[0];
    410         for(int i=1;i<n;++i)
    411         {
    412                 box.mi.setMin(*ppts[i]);
    413                 box.mx.setMax(*ppts[i]);
    414         }
    415         return(box);
     410btDbvtAabbMm box;
     411box.mi=box.mx=*ppts[0];
     412for(int i=1;i<n;++i)
     413        {
     414        box.mi.setMin(*ppts[i]);
     415        box.mx.setMax(*ppts[i]);
     416        }
     417return(box);
    416418}
    417419
    … …  
    419421DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
    420422{
    421         mi-=e;mx+=e;
    422 }
    423 
     423mi-=e;mx+=e;
     424}
     425       
    424426//
    425427DBVT_INLINE void                btDbvtAabbMm::SignedExpand(const btVector3& e)
    426428{
    427         if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
    428         if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
    429         if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
    430 }
    431 
     429if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
     430if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
     431if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
     432}
     433       
    432434//
    433435DBVT_INLINE bool                btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
    434436{
    435         return( (mi.x()<=a.mi.x())&&
     437return( (mi.x()<=a.mi.x())&&
    436438                (mi.y()<=a.mi.y())&&
    437439                (mi.z()<=a.mi.z())&&
    … …  
    444446DBVT_INLINE int         btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
    445447{
    446         btVector3                       pi,px;
    447         switch(s)
     448btVector3                       pi,px;
     449switch(s)
    448450        {
    449451        case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
    450                 pi=btVector3(mx.x(),mx.y(),mx.z());break;
     452                                                pi=btVector3(mx.x(),mx.y(),mx.z());break;
    451453        case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
    452                 pi=btVector3(mi.x(),mx.y(),mx.z());break;
     454                                                pi=btVector3(mi.x(),mx.y(),mx.z());break;
    453455        case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
    454                 pi=btVector3(mx.x(),mi.y(),mx.z());break;
     456                                                pi=btVector3(mx.x(),mi.y(),mx.z());break;
    455457        case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
    456                 pi=btVector3(mi.x(),mi.y(),mx.z());break;
     458                                                pi=btVector3(mi.x(),mi.y(),mx.z());break;
    457459        case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
    458                 pi=btVector3(mx.x(),mx.y(),mi.z());break;
     460                                                pi=btVector3(mx.x(),mx.y(),mi.z());break;
    459461        case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
    460                 pi=btVector3(mi.x(),mx.y(),mi.z());break;
     462                                                pi=btVector3(mi.x(),mx.y(),mi.z());break;
    461463        case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
    462                 pi=btVector3(mx.x(),mi.y(),mi.z());break;
     464                                                pi=btVector3(mx.x(),mi.y(),mi.z());break;
    463465        case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
    464                 pi=btVector3(mi.x(),mi.y(),mi.z());break;
    465         }
    466         if((dot(n,px)+o)<0)             return(-1);
    467         if((dot(n,pi)+o)>=0)    return(+1);
    468         return(0);
     466                                                pi=btVector3(mi.x(),mi.y(),mi.z());break;
     467        }
     468if((dot(n,px)+o)<0)             return(-1);
     469if((dot(n,pi)+o)>=0)    return(+1);
     470return(0);
    469471}
    470472
    … …  
    472474DBVT_INLINE btScalar    btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
    473475{
    474         const btVector3*        b[]={&mx,&mi};
    475         const btVector3         p(      b[(signs>>0)&1]->x(),
    476                 b[(signs>>1)&1]->y(),
    477                 b[(signs>>2)&1]->z());
    478         return(dot(p,v));
     476const btVector3*        b[]={&mx,&mi};
     477const btVector3         p(      b[(signs>>0)&1]->x(),
     478                                                b[(signs>>1)&1]->y(),
     479                                                b[(signs>>2)&1]->z());
     480return(dot(p,v));
    479481}
    480482
    … …  
    482484DBVT_INLINE void                btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
    483485{
    484         for(int i=0;i<3;++i)
    485         {
    486                 if(d[i]<0)
     486for(int i=0;i<3;++i)
     487        {
     488        if(d[i]<0)
    487489                { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
    488490                else
    … …  
    490492        }
    491493}
    492 
     494       
    493495//
    494496DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    495                                                                   const btDbvtAabbMm& b)
     497                                                                        const btDbvtAabbMm& b)
    496498{
    497499#if     DBVT_INT0_IMPL == DBVT_IMPL_SSE
    498         const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
    499                 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
    500         const __int32*  pu((const __int32*)&rt);
    501         return((pu[0]|pu[1]|pu[2])==0);
     500const __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))));
     502const __int32*  pu((const __int32*)&rt);
     503return((pu[0]|pu[1]|pu[2])==0);
    502504#else
    503         return( (a.mi.x()<=b.mx.x())&&
     505return( (a.mi.x()<=b.mx.x())&&
    504506                (a.mx.x()>=b.mi.x())&&
    505507                (a.mi.y()<=b.mx.y())&&
    … …  
    512514//
    513515DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    514                                                                   const btDbvtAabbMm& b,
    515                                                                   const btTransform& xform)
    516 {
    517         const btVector3         d0=xform*b.Center()-a.Center();
    518         const btVector3         d1=d0*xform.getBasis();
    519         btScalar                        s0[2]={0,0};
    520         btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
    521         a.AddSpan(d0,s0[0],s0[1]);
    522         b.AddSpan(d1,s1[0],s1[1]);
    523         if(s0[0]>(s1[1])) return(false);
    524         if(s0[1]<(s1[0])) return(false);
    525         return(true);
     516                                                                        const btDbvtAabbMm& b,
     517                                                                        const btTransform& xform)
     518{
     519const btVector3         d0=xform*b.Center()-a.Center();
     520const btVector3         d1=d0*xform.getBasis();
     521btScalar                        s0[2]={0,0};
     522btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
     523a.AddSpan(d0,s0[0],s0[1]);
     524b.AddSpan(d1,s1[0],s1[1]);
     525if(s0[0]>(s1[1])) return(false);
     526if(s0[1]<(s1[0])) return(false);
     527return(true);
    526528}
    527529
    528530//
    529531DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
    530                                                                   const btVector3& b)
    531 {
    532         return( (b.x()>=a.mi.x())&&
     532                                                                        const btVector3& b)
     533{
     534return( (b.x()>=a.mi.x())&&
    533535                (b.y()>=a.mi.y())&&
    534536                (b.z()>=a.mi.z())&&
    … …  
    538540}
    539541
    540 
    541 
    542 
    543 
    544 //////////////////////////////////////
    545 
    546 
     542//
     543DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
     544                                                                        const btVector3& org,
     545                                                                        const btVector3& invdir,
     546                                                                        const unsigned* signs)
     547{
     548#if 0
     549const btVector3         b0((a.mi-org)*invdir);
     550const btVector3         b1((a.mx-org)*invdir);
     551const btVector3         tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2]));
     552const btVector3         tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2]));
     553const btScalar          tin=btMax(tmin[0],btMax(tmin[1],tmin[2]));
     554const btScalar          tout=btMin(tmax[0],btMin(tmax[1],tmax[2]));
     555return(tin<tout);
     556#else
     557const btVector3*        bounds[2]={&a.mi,&a.mx};
     558btScalar                        txmin=(bounds[  signs[0]]->x()-org[0])*invdir[0];
     559btScalar                        txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
     560const btScalar          tymin=(bounds[  signs[1]]->y()-org[1])*invdir[1];
     561const btScalar          tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
     562if((txmin>tymax)||(tymin>txmax)) return(false);
     563if(tymin>txmin) txmin=tymin;
     564if(tymax<txmax) txmax=tymax;
     565const btScalar          tzmin=(bounds[  signs[2]]->z()-org[2])*invdir[2];
     566const btScalar          tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
     567if((txmin>tzmax)||(tzmin>txmax)) return(false);
     568if(tzmin>txmin) txmin=tzmin;
     569if(tzmax<txmax) txmax=tzmax;
     570return(txmax>0);
     571#endif
     572}
     573       
    547574//
    548575DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
    549                                                                   const btDbvtAabbMm& b)
    550 {
    551         const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
    552         return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
     576                                                                        const btDbvtAabbMm& b)
     577{
     578const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
     579return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
    553580}
    554581
    555582//
    556583DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
    557                                                            const btDbvtAabbMm& a,
    558                                                            const btDbvtAabbMm& b)
     584                                                                const btDbvtAabbMm& a,
     585                                                                const btDbvtAabbMm& b)
    559586{
    560587#if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
    561         static DBVT_ALIGN const unsigned __int32        mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
     588static DBVT_ALIGN const unsigned __int32        mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
    562589        // TODO: the intrinsic version is 11% slower
    563 #if DBVT_USE_INTRINSIC_SSE
     590        #if DBVT_USE_INTRINSIC_SSE
    564591        __m128  omi(_mm_load_ps(o.mi));
    565592        omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
    … …  
    579606        bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
    580607        return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1);
    581 #else
     608        #else
    582609        DBVT_ALIGN __int32      r[1];
    583610        __asm
    584         {
     611                {
    585612                mov             eax,o
    586                         mov             ecx,a
    587                         mov             edx,b
    588                         movaps  xmm0,[eax]
     613                mov             ecx,a
     614                mov             edx,b
     615                movaps  xmm0,[eax]
    589616                movaps  xmm5,mask
    590                         addps   xmm0,[eax+16]   
     617                addps   xmm0,[eax+16]   
    591618                movaps  xmm1,[ecx]
    592619                movaps  xmm2,[edx]
    … …  
    594621                addps   xmm2,[edx+16]
    595622                subps   xmm1,xmm0
    596                         subps   xmm2,xmm0
    597                         andps   xmm1,xmm5
    598                         andps   xmm2,xmm5
    599                         movhlps xmm3,xmm1
    600                         movhlps xmm4,xmm2
    601                         addps   xmm1,xmm3
    602                         addps   xmm2,xmm4
    603                         pshufd  xmm3,xmm1,1
    604                         pshufd  xmm4,xmm2,1
    605                         addss   xmm1,xmm3
    606                         addss   xmm2,xmm4
    607                         cmpless xmm2,xmm1
    608                         movss   r,xmm2
    609         }
     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                }
    610637        return(r[0]&1);
    611 #endif
     638        #endif
    612639#else
    613         return(Proximity(o,a)<Proximity(o,b)?0:1);
     640return(Proximity(o,a)<Proximity(o,b)?0:1);
    614641#endif
    615642}
    … …  
    617644//
    618645DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
    619                                                           const btDbvtAabbMm& b,
    620                                                           btDbvtAabbMm& r)
     646                                                                const btDbvtAabbMm& b,
     647                                                                btDbvtAabbMm& r)
    621648{
    622649#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
    623         __m128  ami(_mm_load_ps(a.mi));
    624         __m128  amx(_mm_load_ps(a.mx));
    625         __m128  bmi(_mm_load_ps(b.mi));
    626         __m128  bmx(_mm_load_ps(b.mx));
    627         ami=_mm_min_ps(ami,bmi);
    628         amx=_mm_max_ps(amx,bmx);
    629         _mm_store_ps(r.mi,ami);
    630         _mm_store_ps(r.mx,amx);
     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));
     654ami=_mm_min_ps(ami,bmi);
     655amx=_mm_max_ps(amx,bmx);
     656_mm_store_ps(r.mi,ami);
     657_mm_store_ps(r.mx,amx);
    631658#else
    632         for(int i=0;i<3;++i)
    633         {
    634                 if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
    635                 if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
     659for(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];
    636663        }
    637664#endif
    … …  
    640667//
    641668DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
    642                                                                  const btDbvtAabbMm& b)
    643 {
    644         return( (a.mi.x()!=b.mi.x())||
     669                                                                        const btDbvtAabbMm& b)
     670{
     671return( (a.mi.x()!=b.mi.x())||
    645672                (a.mi.y()!=b.mi.y())||
    646673                (a.mi.z()!=b.mi.z())||
    … …  
    657684DBVT_PREFIX
    658685inline void             btDbvt::enumNodes(      const btDbvtNode* root,
    659                                                                   DBVT_IPOLICY)
    660 {
    661         DBVT_CHECKTYPE
    662                 policy.Process(root);
    663         if(root->isinternal())
    664         {
    665                 enumNodes(root->childs[0],policy);
    666                 enumNodes(root->childs[1],policy);
     686                                                                        DBVT_IPOLICY)
     687{
     688DBVT_CHECKTYPE
     689policy.Process(root);
     690if(root->isinternal())
     691        {
     692        enumNodes(root->childs[0],policy);
     693        enumNodes(root->childs[1],policy);
    667694        }
    668695}
    … …  
    671698DBVT_PREFIX
    672699inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
    673                                                                    DBVT_IPOLICY)
    674 {
    675         DBVT_CHECKTYPE
    676                 if(root->isinternal())
    677                 {
    678                         enumLeaves(root->childs[0],policy);
    679                         enumLeaves(root->childs[1],policy);
    680                 }
    681                 else
    682                 {
    683                         policy.Process(root);
    684                 }
     700                                                                        DBVT_IPOLICY)
     701{
     702DBVT_CHECKTYPE
     703if(root->isinternal())
     704        {
     705        enumLeaves(root->childs[0],policy);
     706        enumLeaves(root->childs[1],policy);
     707        }
     708        else
     709        {
     710        policy.Process(root);
     711        }
    685712}
    686713
    … …  
    688715DBVT_PREFIX
    689716inline void             btDbvt::collideTT(      const btDbvtNode* root0,
    690                                                                   const btDbvtNode* root1,
    691                                                                   DBVT_IPOLICY)
    692 {
    693         DBVT_CHECKTYPE
    694                 if(root0&&root1)
    695                 {
    696                         btAlignedObjectArray<sStkNN>    stack;
    697                         int                                                             depth=1;
    698                         int                                                             treshold=DOUBLE_STACKSIZE-4;
    699                         stack.resize(DOUBLE_STACKSIZE);
    700                         stack[0]=sStkNN(root0,root1);
    701                         do      {               
    702                                 sStkNN  p=stack[--depth];
    703                                 if(depth>treshold)
    704                                 {
    705                                         stack.resize(stack.size()*2);
    706                                         treshold=stack.size()-4;
    707                                 }
    708                                 if(p.a==p.b)
    709                                 {
    710                                         if(p.a->isinternal())
     717                                                                        const btDbvtNode* root1,
     718                                                                        DBVT_IPOLICY)
     719{
     720DBVT_CHECKTYPE
     721if(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)
     731                        {
     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())
    711749                                        {
    712                                                 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
    713                                                 stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
    714                                                 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
    715                                         }
    716                                 }
    717                                 else if(Intersect(p.a->volume,p.b->volume))
    718                                 {
    719                                         if(p.a->isinternal())
    720                                         {
    721                                                 if(p.b->isinternal())
    722                                                 {
    723                                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
    724                                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
    725                                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
    726                                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
    727                                                 }
    728                                                 else
    729                                                 {
    730                                                         stack[depth++]=sStkNN(p.a->childs[0],p.b);
    731                                                         stack[depth++]=sStkNN(p.a->childs[1],p.b);
    732                                                 }
     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]);
    733754                                        }
    734755                                        else
    735756                                        {
    736                                                 if(p.b->isinternal())
    737                                                 {
    738                                                         stack[depth++]=sStkNN(p.a,p.b->childs[0]);
    739                                                         stack[depth++]=sStkNN(p.a,p.b->childs[1]);
    740                                                 }
    741                                                 else
    742                                                 {
    743                                                         policy.Process(p.a,p.b);
    744                                                 }
     757                                        stack[depth++]=sStkNN(p.a->childs[0],p.b);
     758                                        stack[depth++]=sStkNN(p.a->childs[1],p.b);
    745759                                        }
    746760                                }
    747                         } while(depth);
    748                 }
    749 }
    750 
    751 //
    752 DBVT_PREFIX
    753 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
    754                                                                   const btDbvtNode* root1,
    755                                                                   const btTransform& xform,
    756                                                                   DBVT_IPOLICY)
    757 {
    758         DBVT_CHECKTYPE
    759                 if(root0&&root1)
    760                 {
    761                         btAlignedObjectArray<sStkNN>    stack;
    762                         int                                                             depth=1;
    763                         int                                                             treshold=DOUBLE_STACKSIZE-4;
    764                         stack.resize(DOUBLE_STACKSIZE);
    765                         stack[0]=sStkNN(root0,root1);
    766                         do      {
    767                                 sStkNN  p=stack[--depth];
    768                                 if(Intersect(p.a->volume,p.b->volume,xform))
    769                                 {
    770                                         if(depth>treshold)
     761                                else
     762                                {
     763                                if(p.b->isinternal())
    771764                                        {
    772                                                 stack.resize(stack.size()*2);
    773                                                 treshold=stack.size()-4;
    774                                         }
    775                                         if(p.a->isinternal())
    776                                         {
    777                                                 if(p.b->isinternal())
    778                                                 {                                       
    779                                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
    780                                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
    781                                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
    782                                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
    783                                                 }
    784                                                 else
    785                                                 {
    786                                                         stack[depth++]=sStkNN(p.a->childs[0],p.b);
    787                                                         stack[depth++]=sStkNN(p.a->childs[1],p.b);
    788                                                 }
     765                                        stack[depth++]=sStkNN(p.a,p.b->childs[0]);
     766                                        stack[depth++]=sStkNN(p.a,p.b->childs[1]);
    789767                                        }
    790768                                        else
    791769                                        {
    792                                                 if(p.b->isinternal())
    793                                                 {
    794                                                         stack[depth++]=sStkNN(p.a,p.b->childs[0]);
    795                                                         stack[depth++]=sStkNN(p.a,p.b->childs[1]);
    796                                                 }
    797                                                 else
    798                                                 {
    799                                                         policy.Process(p.a,p.b);
    800                                                 }
     770                                        policy.Process(p.a,p.b);
    801771                                        }
    802772                                }
    803                         } while(depth);
    804                 }
     773                        }
     774                } while(depth);
     775        }
    805776}
    806777
    … …  
    808779DBVT_PREFIX
    809780inline void             btDbvt::collideTT(      const btDbvtNode* root0,
    810                                                                   const btTransform& xform0,
    811                                                                   const btDbvtNode* root1,
    812                                                                   const btTransform& xform1,
    813                                                                   DBVT_IPOLICY)
    814 {
    815         const btTransform       xform=xform0.inverse()*xform1;
    816         collideTT(root0,root1,xform,policy);
    817 }
    818 
    819 //
    820 DBVT_PREFIX
    821 inline void             btDbvt::collideTV(      const btDbvtNode* root,
    822                                                                   const btDbvtVolume& vol,
    823                                                                   DBVT_IPOLICY)
    824 {
    825         DBVT_CHECKTYPE
    826                 if(root)
    827                 {
    828                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
    829                         btAlignedObjectArray<const btDbvtNode*> stack;
    830                         stack.reserve(SIMPLE_STACKSIZE);
    831                         stack.push_back(root);
    832                         do      {
    833                                 const btDbvtNode*       n=stack[stack.size()-1];
    834                                 stack.pop_back();
    835                                 if(Intersect(n->volume,volume))
    836                                 {
    837                                         if(n->isinternal())
    838                                         {
    839                                                 stack.push_back(n->childs[0]);
    840                                                 stack.push_back(n->childs[1]);
     781                                                                        const btDbvtNode* root1,
     782                                                                        const btTransform& xform,
     783                                                                        DBVT_IPOLICY)
     784{
     785DBVT_CHECKTYPE
     786if(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]);
    841810                                        }
    842811                                        else
    843812                                        {
    844                                                 policy.Process(n);
     813                                        stack[depth++]=sStkNN(p.a->childs[0],p.b);
     814                                        stack[depth++]=sStkNN(p.a->childs[1],p.b);
    845815                                        }
    846816                                }
    847                         } while(stack.size()>0);
    848                 }
    849 }
    850 
    851 
    852 //
    853 DBVT_PREFIX
    854 inline void             btDbvt::rayTest(        const btDbvtNode* root,
    855                                                                 const btVector3& rayFrom,
    856                                                                 const btVector3& rayTo,
    857                                                                 DBVT_IPOLICY)
    858 {
    859         DBVT_CHECKTYPE
    860                 if(root)
    861                 {
    862                         btVector3 rayDir = (rayTo-rayFrom);
    863                         rayDir.normalize ();
    864 
    865                         ///what about division by zero? --> just set rayDirection[i] to INF/1e30
    866                         btVector3 rayDirectionInverse;
    867                         rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
    868                         rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
    869                         rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
    870                         unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
    871 
    872 
    873                         btVector3 resultNormal;
    874 
    875 
    876                         btAlignedObjectArray<const btDbvtNode*> stack;
    877                         stack.reserve(SIMPLE_STACKSIZE);
    878                         stack.push_back(root);
    879                         do      {
    880                                 const btDbvtNode*       node=stack[stack.size()-1];
    881                                 stack.pop_back();
    882 
    883                                 btVector3 bounds[2] = {node->volume.Mins(),node->volume.Maxs()};
    884                                 btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
    885                                 btScalar tmin=1.f,lambda_min=0.f;
    886                                 bool result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
    887 
    888 #ifdef COMPARE_BTRAY_AABB2
    889                                 btScalar param=1.f;
    890                                 bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
    891                                 btAssert(result1 == result2);
    892 #endif //TEST_BTRAY_AABB2
    893 
    894                                 if(result1)
    895                                 {
    896                                         if(node->isinternal())
     817                                else
     818                                {
     819                                if(p.b->isinternal())
    897820                                        {
    898                                                 stack.push_back(node->childs[0]);
    899                                                 stack.push_back(node->childs[1]);
     821                                        stack[depth++]=sStkNN(p.a,p.b->childs[0]);
     822                                        stack[depth++]=sStkNN(p.a,p.b->childs[1]);
    900823                                        }
    901824                                        else
    902825                                        {
    903                                                 policy.Process(node);
     826                                        policy.Process(p.a,p.b);
    904827                                        }
    905828                                }
    906                         } while(stack.size());
    907                 }
     829                        }
     830                } while(depth);
     831        }
     832}
     833
     834//
     835DBVT_PREFIX
     836inline void             btDbvt::collideTT(      const btDbvtNode* root0,
     837                                                                        const btTransform& xform0,
     838                                                                        const btDbvtNode* root1,
     839                                                                        const btTransform& xform1,
     840                                                                        DBVT_IPOLICY)
     841{
     842const btTransform       xform=xform0.inverse()*xform1;
     843collideTT(root0,root1,xform,policy);
     844}
     845
     846//
     847DBVT_PREFIX
     848inline void             btDbvt::collideTV(      const btDbvtNode* root,
     849                                                                        const btDbvtVolume& vol,
     850                                                                        DBVT_IPOLICY)
     851{
     852DBVT_CHECKTYPE
     853if(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//
     879DBVT_PREFIX
     880inline void             btDbvt::collideRAY(     const btDbvtNode* root,
     881                                                                        const btVector3& origin,
     882                                                                        const btVector3& direction,
     883                                                                        DBVT_IPOLICY)
     884{
     885DBVT_CHECKTYPE
     886if(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        }
    908915}
    909916
    … …  
    916923                                                                        DBVT_IPOLICY)
    917924{
    918         DBVT_CHECKTYPE
    919                 if(root)
     925DBVT_CHECKTYPE
     926if(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)
    920933                {
    921                         const int                                               inside=(1<<count)-1;
    922                         btAlignedObjectArray<sStkNP>    stack;
    923                         int                                                             signs[sizeof(unsigned)*8];
    924                         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
    925                         for(int i=0;i<count;++i)
    926                         {
    927                                 signs[i]=       ((normals[i].x()>=0)?1:0)+
     934                signs[i]=       ((normals[i].x()>=0)?1:0)+
    928935                                        ((normals[i].y()>=0)?2:0)+
    929936                                        ((normals[i].z()>=0)?4:0);
     937                }
     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                                }
    930955                        }
    931                         stack.reserve(SIMPLE_STACKSIZE);
    932                         stack.push_back(sStkNP(root,0));
    933                         do      {
    934                                 sStkNP  se=stack[stack.size()-1];
    935                                 bool    out=false;
    936                                 stack.pop_back();
    937                                 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
    938                                 {
    939                                         if(0==(se.mask&j))
     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        }
     970}
     971
     972//
     973DBVT_PREFIX
     974inline 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{
     982DBVT_CHECKTYPE
     983if(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)
     995                {
     996                signs[i]=       ((normals[i].x()>=0)?1:0)+
     997                                        ((normals[i].y()>=0)?2:0)+
     998                                        ((normals[i].z()>=0)?4:0);
     999                }
     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))
    9401014                                        {
    941                                                 const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
    942                                                 switch(side)
     1015                                        const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
     1016                                        switch(side)
    9431017                                                {
    9441018                                                case    -1:     out=true;break;
    … …  
    9471021                                        }
    9481022                                }
    949                                 if(!out)
    950                                 {
    951                                         if((se.mask!=inside)&&(se.node->isinternal()))
     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))
    9521035                                        {
    953                                                 stack.push_back(sStkNP(se.node->childs[0],se.mask));
    954                                                 stack.push_back(sStkNP(se.node->childs[1],se.mask));
     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]);
    9551054                                        }
    9561055                                        else
    9571056                                        {
    958                                                 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
     1057                                        stack.push_back(allocate(ifree,stock,nes[q]));
     1058                                        stack.push_back(allocate(ifree,stock,nes[1-q]));
    9591059                                        }
    9601060                                }
    961                         } while(stack.size());
    962                 }
    963 }
    964 
    965 //
    966 DBVT_PREFIX
    967 inline void             btDbvt::collideOCL(     const btDbvtNode* root,
    968                                                                    const btVector3* normals,
    969                                                                    const btScalar* offsets,
    970                                                                    const btVector3& sortaxis,
    971                                                                    int count,
    972                                                                    DBVT_IPOLICY,
    973                                                                    bool fsort)
    974 {
    975         DBVT_CHECKTYPE
    976                 if(root)
    977                 {
    978                         const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
    979                                 (sortaxis[1]>=0?2:0)+
    980                                 (sortaxis[2]>=0?4:0);
    981                         const int                                               inside=(1<<count)-1;
    982                         btAlignedObjectArray<sStkNPS>   stock;
    983                         btAlignedObjectArray<int>               ifree;
    984                         btAlignedObjectArray<int>               stack;
    985                         int                                                             signs[sizeof(unsigned)*8];
    986                         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
    987                         for(int i=0;i<count;++i)
    988                         {
    989                                 signs[i]=       ((normals[i].x()>=0)?1:0)+
    990                                         ((normals[i].y()>=0)?2:0)+
    991                                         ((normals[i].z()>=0)?4:0);
     1061                                else
     1062                                {
     1063                                policy.Process(se.node,se.value);
     1064                                }
    9921065                        }
    993                         stock.reserve(SIMPLE_STACKSIZE);
    994                         stack.reserve(SIMPLE_STACKSIZE);
    995                         ifree.reserve(SIMPLE_STACKSIZE);
    996                         stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
    997                         do      {
    998                                 const int       id=stack[stack.size()-1];
    999                                 sStkNPS         se=stock[id];
    1000                                 stack.pop_back();ifree.push_back(id);
    1001                                 if(se.mask!=inside)
    1002                                 {
    1003                                         bool    out=false;
    1004                                         for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
    1005                                         {
    1006                                                 if(0==(se.mask&j))
    1007                                                 {
    1008                                                         const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
    1009                                                         switch(side)
    1010                                                         {
    1011                                                         case    -1:     out=true;break;
    1012                                                         case    +1:     se.mask|=j;break;
    1013                                                         }
    1014                                                 }
    1015                                         }
    1016                                         if(out) continue;
    1017                                 }
    1018                                 if(policy.Descent(se.node))
    1019                                 {
    1020                                         if(se.node->isinternal())
    1021                                         {
    1022                                                 const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
    1023                                                 sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
    1024                                                         sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
    1025                                                 const int       q=nes[0].value<nes[1].value?1:0;                               
    1026                                                 int                     j=stack.size();
    1027                                                 if(fsort&&(j>0))
    1028                                                 {
    1029                                                         /* Insert 0     */
    1030                                                         j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
    1031                                                         stack.push_back(0);
    1032 #if DBVT_USE_MEMMOVE
    1033                                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
    1034 #else
    1035                                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
    1036 #endif
    1037                                                         stack[j]=allocate(ifree,stock,nes[q]);
    1038                                                         /* Insert 1     */
    1039                                                         j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
    1040                                                         stack.push_back(0);
    1041 #if DBVT_USE_MEMMOVE
    1042                                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
    1043 #else
    1044                                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
    1045 #endif
    1046                                                         stack[j]=allocate(ifree,stock,nes[1-q]);
    1047                                                 }
    1048                                                 else
    1049                                                 {
    1050                                                         stack.push_back(allocate(ifree,stock,nes[q]));
    1051                                                         stack.push_back(allocate(ifree,stock,nes[1-q]));
    1052                                                 }
    1053                                         }
    1054                                         else
    1055                                         {
    1056                                                 policy.Process(se.node,se.value);
    1057                                         }
    1058                                 }
    1059                         } while(stack.size());
    1060                 }
     1066                } while(stack.size());
     1067        }
    10611068}
    10621069
    … …  
    10641071DBVT_PREFIX
    10651072inline void             btDbvt::collideTU(      const btDbvtNode* root,
    1066                                                                   DBVT_IPOLICY)
    1067 {
    1068         DBVT_CHECKTYPE
    1069                 if(root)
    1070                 {
    1071                         btAlignedObjectArray<const btDbvtNode*> stack;
    1072                         stack.reserve(SIMPLE_STACKSIZE);
    1073                         stack.push_back(root);
    1074                         do      {
    1075                                 const btDbvtNode*       n=stack[stack.size()-1];
    1076                                 stack.pop_back();
    1077                                 if(policy.Descent(n))
    1078                                 {
    1079                                         if(n->isinternal())
    1080                                         { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
    1081                                         else
    1082                                         { policy.Process(n); }
    1083                                 }
    1084                         } while(stack.size()>0);
    1085                 }
     1073                                                                        DBVT_IPOLICY)
     1074{
     1075DBVT_CHECKTYPE
     1076if(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        }
    10861093}
    10871094

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

    r1963 r1972  
    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;
     55item->links[0]=0;
     56item->links[1]=list;
     57if(list) list->links[0]=item;
     58list=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];
     65if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
     66if(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);
     73int     n=0;
     74while(root) { ++n;root=root->links[1]; }
     75return(n);
    7676}
    7777
    … …  
    8080static inline void      clear(T& value)
    8181{
    82         static const struct ZeroDummy : T {} zerodummy;
    83         value=zerodummy;
     82static const struct ZeroDummy : T {} zerodummy;
     83value=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);
     93btDbvtBroadphase*       pbp;
     94btDbvtProxy*            proxy;
     95                btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
     96void    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        }
     109void    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;
     122m_deferedcollide        =       false;
     123m_needcleanup           =       true;
     124m_releasepaircache      =       (paircache!=0)?false:true;
     125m_prediction            =       1/(btScalar)2;
     126m_stageCurrent          =       0;
     127m_fixedleft                     =       0;
     128m_fupdates                      =       1;
     129m_dupdates                      =       0;
     130m_cupdates                      =       10;
     131m_newpairs                      =       1;
     132m_updates_call          =       0;
     133m_updates_done          =       0;
     134m_updates_ratio         =       0;
     135m_paircache                     =       paircache?
     136                                                paircache       :
     137                                                new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
     138m_gid                           =       0;
     139m_pid                           =       0;
     140m_cid                           =       0;
     141for(int i=0;i<=STAGECOUNT;++i)
     142        {
     143        m_stageRoots[i]=0;
    144144        }
    145145#if DBVT_BP_PROFILE
    146         clear(m_profiling);
     146clear(m_profiling);
    147147#endif
    148148}
    … …  
    151151btDbvtBroadphase::~btDbvtBroadphase()
    152152{
    153         if(m_releasepaircache)
    154         {
    155                 m_paircache->~btOverlappingPairCache();
    156                 btAlignedFree(m_paircache);
    157         }
     153if(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(  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                 btDbvt::collideTV(m_sets[0].m_root,aabb,collider);
    186                 btDbvt::collideTV(m_sets[1].m_root,aabb,collider);
    187         }
    188         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{
     170btDbvtProxy*            proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  userPtr,
     171                                                                                                                                                                        collisionFilterGroup,
     172                                                                                                                                                                        collisionFilterMask);
     173proxy->aabb                     =       btDbvtVolume::FromMM(aabbMin,aabbMax);
     174proxy->stage            =       m_stageCurrent;
     175proxy->m_uniqueId       =       ++m_gid;
     176proxy->leaf                     =       m_sets[0].insert(proxy->aabb,proxy);
     177listappend(proxy,m_stageRoots[m_stageCurrent]);
     178if(!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        }
     185return(proxy);
    189186}
    190187
    191188//
    192189void                                                    btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
    193                                                                                                                            btDispatcher* dispatcher)
    194 {
    195         btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
     190                                                                                                                                btDispatcher* dispatcher)
     191{
     192btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
     193if(proxy->stage==STAGECOUNT)
     194        m_sets[1].remove(proxy->leaf);
     195        else
     196        m_sets[0].remove(proxy->leaf);
     197listremove(proxy,m_stageRoots[proxy->stage]);
     198m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
     199btAlignedFree(proxy);
     200m_needcleanup=true;
     201}
     202
     203//
     204void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
     205                                                                                                                                const btVector3& aabbMin,
     206                                                                                                                                const btVector3& aabbMax,
     207                                                                                                                                btDispatcher* /*dispatcher*/)
     208{
     209btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
     210ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
     211#if DBVT_BP_PREVENTFALSEUPDATE
     212if(NotEqual(aabb,proxy->leaf->volume))
     213#endif
     214        {
     215        bool    docollide=false;
    196216        if(proxy->stage==STAGECOUNT)
     217                {/* fixed -> dynamic set        */
    197218                m_sets[1].remove(proxy->leaf);
    198         else
    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 
    206 void    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 
    213 void    btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
    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].rayTest(      m_sets[0].m_root,
    233                 rayFrom,
    234                 rayTo,
    235                 callback);
    236 
    237         m_sets[1].rayTest(      m_sets[1].m_root,
    238                 rayFrom,
    239                 rayTo,
    240                 callback);
    241 
    242 }
    243 
    244 //
    245 void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
    246                                                                                                                   const btVector3& aabbMin,
    247                                                                                                                   const btVector3& aabbMax,
    248                                                                                                                   btDispatcher* /*dispatcher*/)
    249 {
    250         btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
    251         ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
    252 #if DBVT_BP_PREVENTFALSEUPDATE
    253         if(NotEqual(aabb,proxy->leaf->volume))
    254 #endif
    255         {
    256                 bool    docollide=false;
    257                 if(proxy->stage==STAGECOUNT)
    258                 {/* fixed -> dynamic set        */
    259                         m_sets[1].remove(proxy->leaf);
    260                         proxy->leaf=m_sets[0].insert(aabb,proxy);
    261                         docollide=true;
     219                proxy->leaf=m_sets[0].insert(aabb,proxy);
     220                docollide=true;
    262221                }
    263222                else
    264223                {/* dynamic set                         */
    265                         ++m_updates_call;
    266                         if(Intersect(proxy->leaf->volume,aabb))
     224                ++m_updates_call;
     225                if(Intersect(proxy->leaf->volume,aabb))
    267226                        {/* Moving                              */
    268 
    269                                 const btVector3 delta=aabbMin-proxy->m_aabbMin;
    270                                 btVector3               velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
    271                                 if(delta[0]<0) velocity[0]=-velocity[0];
    272                                 if(delta[1]<0) velocity[1]=-velocity[1];
    273                                 if(delta[2]<0) velocity[2]=-velocity[2];
    274                                 if      (
    275 #ifdef DBVT_BP_MARGIN                           
    276                                         m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
    277 #else
    278                                         m_sets[0].update(proxy->leaf,aabb,velocity)
    279 #endif
    280                                         )
     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                                )
    281239                                {
    282                                         ++m_updates_done;
    283                                         docollide=true;
     240                                ++m_updates_done;
     241                                docollide=true;
    284242                                }
    285243                        }
    286244                        else
    287245                        {/* Teleporting                 */
    288                                 m_sets[0].update(proxy->leaf,aabb);
    289                                 ++m_updates_done;
    290                                 docollide=true;
     246                        m_sets[0].update(proxy->leaf,aabb);
     247                        ++m_updates_done;
     248                        docollide=true;
    291249                        }       
    292250                }
    293                 listremove(proxy,m_stageRoots[proxy->stage]);
    294                 proxy->m_aabbMin = aabbMin;
    295                 proxy->m_aabbMax = aabbMax;
    296                 proxy->stage    =       m_stageCurrent;
    297                 listappend(proxy,m_stageRoots[m_stageCurrent]);
    298                 if(docollide)
    299                 {
    300                         m_needcleanup=true;
    301                         if(!m_deferedcollide)
     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)
    302259                        {
    303                                 btDbvtTreeCollider      collider(this);
    304                                 btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
    305                                 btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
     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);
    306263                        }
    307264                }       
    … …  
    312269void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
    313270{
    314         collide(dispatcher);
     271collide(dispatcher);
    315272#if DBVT_BP_PROFILE
    316         if(0==(m_pid%DBVT_BP_PROFILING_RATE))
     273if(0==(m_pid%DBVT_BP_PROFILING_RATE))
    317274        {       
    318                 printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
    319                 unsigned int    total=m_profiling.m_total;
    320                 if(total<=0) total=1;
    321                 printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
    322                 printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
    323                 printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
    324                 printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
    325                 const unsigned long     sum=m_profiling.m_ddcollide+
    326                         m_profiling.m_fdcollide+
    327                         m_profiling.m_cleanup;
    328                 printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
    329                 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));
    330                 clear(m_profiling);
    331                 m_clock.reset();
     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();
    332289        }
    333290#endif
    … …  
    337294void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
    338295{
    339         SPC(m_profiling.m_total);
    340         /* optimize                             */
    341         m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
    342         if(m_fixedleft)
    343         {
    344                 const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
    345                 m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
    346                 m_fixedleft=btMax<int>(0,m_fixedleft-count);
    347         }
    348         /* dynamic -> fixed set */
    349         m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
    350         btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
    351         if(current)
    352         {
    353                 btDbvtTreeCollider      collider(this);
    354                 do      {
    355                         btDbvtProxy*    next=current->links[1];
    356                         listremove(current,m_stageRoots[current->stage]);
    357                         listappend(current,m_stageRoots[STAGECOUNT]);
    358 #if DBVT_BP_ACCURATESLEEPING
    359                         m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
    360                         collider.proxy=current;
    361                         btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
    362                         btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
    363 #endif
    364                         m_sets[0].remove(current->leaf);
    365                         ATTRIBUTE_ALIGNED16(btDbvtVolume)       curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
    366                         current->leaf   =       m_sets[1].insert(curAabb,current);
    367                         current->stage  =       STAGECOUNT;     
    368                         current                 =       next;
     296SPC(m_profiling.m_total);
     297/* optimize                             */
     298m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
     299if(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 */
     306m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
     307btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
     308if(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;
    369325                } while(current);
    370                 m_fixedleft=m_sets[1].m_leaves;
    371                 m_needcleanup=true;
    372         }
    373         /* collide dynamics             */
    374         {
    375                 btDbvtTreeCollider      collider(this);
    376                 if(m_deferedcollide)
    377                 {
    378                         SPC(m_profiling.m_fdcollide);
    379                         btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
    380                 }
    381                 if(m_deferedcollide)
    382                 {
    383                         SPC(m_profiling.m_ddcollide);
    384                         btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
    385                 }
    386         }
    387         /* clean up                             */
    388         if(m_needcleanup)
    389         {
    390                 SPC(m_profiling.m_cleanup);
    391                 btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
    392                 if(pairs.size()>0)
    393                 {
    394                         const int       ci=pairs.size();
    395                         int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
    396                         for(int i=0;i<ni;++i)
     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                             */
     344if(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)
    397353                        {
    398                                 btBroadphasePair&       p=pairs[(m_cid+i)%ci];
    399                                 btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
    400                                 btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
    401                                 if(!Intersect(pa->leaf->volume,pb->leaf->volume))
     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))
    402358                                {
    403 #if DBVT_BP_SORTPAIRS
    404                                         if(pa>pb) btSwap(pa,pb);
    405 #endif
    406                                         m_paircache->removeOverlappingPair(pa,pb,dispatcher);
    407                                         --ni;--i;
     359                                #if DBVT_BP_SORTPAIRS
     360                                if(pa>pb) btSwap(pa,pb);
     361                                #endif
     362                                m_paircache->removeOverlappingPair(pa,pb,dispatcher);
     363                                --ni;--i;
    408364                                }
    409365                        }
    410                         if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
    411                 }
    412         }
    413         ++m_pid;
    414         m_newpairs=1;
    415         m_needcleanup=false;
    416         if(m_updates_call>0)
     366                if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
     367                }
     368        }
     369++m_pid;
     370m_newpairs=1;
     371m_needcleanup=false;
     372if(m_updates_call>0)
    417373        { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
    418374        else
    419375        { m_updates_ratio=0; }
    420         m_updates_done/=2;
    421         m_updates_call/=2;
     376m_updates_done/=2;
     377m_updates_call/=2;
    422378}
    423379
    … …  
    425381void                                                    btDbvtBroadphase::optimize()
    426382{
    427         m_sets[0].optimizeTopDown();
    428         m_sets[1].optimizeTopDown();
     383m_sets[0].optimizeTopDown();
     384m_sets[1].optimizeTopDown();
    429385}
    430386
    … …  
    432388btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
    433389{
    434         return(m_paircache);
     390return(m_paircache);
    435391}
    436392
    … …  
    438394const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
    439395{
    440         return(m_paircache);
     396return(m_paircache);
    441397}
    442398
    … …  
    447403        ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
    448404
    449         if(!m_sets[0].empty())
    450                 if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
    451                         m_sets[1].m_root->volume,bounds);
    452                 else
    453                         bounds=m_sets[0].m_root->volume;
    454         else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
    455         else
    456                 bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
    457         aabbMin=bounds.Mins();
    458         aabbMax=bounds.Maxs();
     405if(!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;
     410else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
     411                                                        else
     412                                                        bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
     413aabbMin=bounds.Mins();
     414aabbMax=bounds.Maxs();
    459415}
    460416
    … …  
    467423
    468424struct  btBroadphaseBenchmark
    469 {
     425        {
    470426        struct  Experiment
    471         {
     427                {
    472428                const char*                     name;
    473429                int                                     object_count;
    … …  
    477433                btScalar                        speed;
    478434                btScalar                        amplitude;
    479         };
     435                };
    480436        struct  Object
    481         {
     437                {
    482438                btVector3                       center;
    483439                btVector3                       extents;
    … …  
    485441                btScalar                        time;
    486442                void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
    487                 {
     443                        {
    488444                        time            +=      speed;
    489445                        center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
    490                                 btSin(time)*amplitude/2;
     446                                                        btSin(time)*amplitude/2;
    491447                        center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
    492                                 btSin(time)*amplitude;
     448                                                        btSin(time)*amplitude;
    493449                        center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
    494450                        pbi->setAabb(proxy,center-extents,center+extents,0);
    495                 }
    496         };
     451                        }
     452                };
    497453        static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
    498454        static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
    499455        static void             OutputTime(const char* name,btClock& c,unsigned count=0)
    500         {
     456                {
    501457                const unsigned long     us=c.getTimeMicroseconds();
    502458                const unsigned long     ms=(us+500)/1000;
    … …  
    504460                if(count>0)
    505461                        printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
    506                 else
     462                        else
    507463                        printf("%s : %u us (%u ms)\r\n",name,us,ms);
    508         }
    509 };
     464                }
     465        };
    510466
    511467void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
    512468{
    513         static const btBroadphaseBenchmark::Experiment          experiments[]=
    514         {
    515                 {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
    516                 /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
    517                 {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
     469static 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},*/
    518474        };
    519         static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
    520         btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
    521         btClock                                                                                                 wallclock;
    522         /* Begin                        */
    523         for(int iexp=0;iexp<nexperiments;++iexp)
    524         {
    525                 const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
    526                 const int                                                                       object_count=experiment.object_count;
    527                 const int                                                                       update_count=(object_count*experiment.update_count)/100;
    528                 const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
    529                 const btScalar                                                          speed=experiment.speed;
    530                 const btScalar                                                          amplitude=experiment.amplitude;
    531                 printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
    532                 printf("\tObjects: %u\r\n",object_count);
    533                 printf("\tUpdate: %u\r\n",update_count);
    534                 printf("\tSpawn: %u\r\n",spawn_count);
    535                 printf("\tSpeed: %f\r\n",speed);
    536                 printf("\tAmplitude: %f\r\n",amplitude);
    537                 srand(180673);
    538                 /* Create objects       */
    539                 wallclock.reset();
    540                 objects.reserve(object_count);
    541                 for(int i=0;i<object_count;++i)
    542                 {
    543                         btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
    544                         po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
    545                         po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
    546                         po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
    547                         po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
    548                         po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
    549                         po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
    550                         po->time=btBroadphaseBenchmark::UnitRand()*2000;
    551                         po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
    552                         objects.push_back(po);
    553                 }
    554                 btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
    555                 /* First update         */
    556                 wallclock.reset();
    557                 for(int i=0;i<objects.size();++i)
    558                 {
    559                         objects[i]->update(speed,amplitude,pbi);
    560                 }
    561                 btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
    562                 /* Updates                      */
    563                 wallclock.reset();
    564                 for(int i=0;i<experiment.iterations;++i)
    565                 {
    566                         for(int j=0;j<update_count;++j)
     475static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
     476btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
     477btClock                                                                                                 wallclock;
     478/* Begin                        */
     479for(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)
    567523                        {                               
    568                                 objects[j]->update(speed,amplitude,pbi);
     524                        objects[j]->update(speed,amplitude,pbi);
    569525                        }
    570                         pbi->calculateOverlappingPairs(0);
    571                 }
    572                 btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
    573                 /* Clean up                     */
    574                 wallclock.reset();
    575                 for(int i=0;i<objects.size();++i)
    576                 {
    577                         pbi->destroyProxy(objects[i]->proxy,0);
    578                         delete objects[i];
    579                 }
    580                 objects.resize(0);
    581                 btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
     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);
    582538        }
    583539

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

    r1963 r1972  
    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(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
    50         btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
     43/* Fields               */
     44btDbvtAabbMm    aabb;
     45btDbvtNode*             leaf;
     46btDbvtProxy*    links[2];
     47int                             stage;
     48/* ctor                 */
     49btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
     50        btBroadphaseProxy(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               */
     64enum    {
    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               */
     70btDbvt                                  m_sets[2];                                      // Dbvt sets
     71btDbvtProxy*                    m_stageRoots[STAGECOUNT+1];     // Stages list
     72btOverlappingPairCache* m_paircache;                            // Pair cache
     73btScalar                                m_prediction;                           // Velocity prediction
     74int                                             m_stageCurrent;                         // Current stage
     75int                                             m_fupdates;                                     // % of fixed updates per frame
     76int                                             m_dupdates;                                     // % of dynamic updates per frame
     77int                                             m_cupdates;                                     // % of cleanup updates per frame
     78int                                             m_newpairs;                                     // Number of pairs created
     79int                                             m_fixedleft;                            // Fixed optimization left
     80unsigned                                m_updates_call;                         // Number of updates call
     81unsigned                                m_updates_done;                         // Number of updates done
     82btScalar                                m_updates_ratio;                        // m_updates_done/m_updates_call
     83int                                             m_pid;                                          // Parse id
     84int                                             m_cid;                                          // Cleanup index
     85int                                             m_gid;                                          // Gen id
     86bool                                    m_releasepaircache;                     // Release pair cache on delete
     87bool                                    m_deferedcollide;                       // Defere dynamic/static collision to collide call
     88bool                                    m_needcleanup;                          // Need to run cleanup?
    8989#if DBVT_BP_PROFILE
    90         btClock                                 m_clock;
    91         struct  {
     90btClock                                 m_clock;
     91struct  {
    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         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
    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*);
     99/* Methods              */
     100btDbvtBroadphase(btOverlappingPairCache* paircache=0);
     101~btDbvtBroadphase();
     102void                                                    collide(btDispatcher* dispatcher);
     103void                                                    optimize();
     104/* btBroadphaseInterface Implementation */
     105btBroadphaseProxy*                              createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
     106void                                                    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
     107void                                                    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
     108void                                                    calculateOverlappingPairs(btDispatcher* dispatcher);
     109btOverlappingPairCache*                 getOverlappingPairCache();
     110const btOverlappingPairCache*   getOverlappingPairCache() const;
     111void                                                    getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
     112void                                                    printStats();
     113static void                                             benchmark(btBroadphaseInterface*);
    117114};
    118115

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

    r1963 r1972  
    150150
    151151
    152 void    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 
    159 void    btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
    160 {
    161         for (int i=0;i<m_multiSapProxies.size();i++)
    162         {
    163                 rayCallback.process(m_multiSapProxies[i]);
    164         }
    165 }
    166 
    167 
    168152//#include <stdio.h>
    169153

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

    r1963 r1972  
    7373*/
    7474                btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
    75                         :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
     75                        :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
    7676                        m_aabbMin(aabbMin),
    7777                        m_aabbMax(aabbMax),
    … …  
    109109        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    110110        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
    111         virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
    112 
    113         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
    114111
    115112        void    addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*        childBroadphase);

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

    r1963 r1972  
    1919#include "LinearMath/btIDebugDraw.h"
    2020
    21 #define RAYAABB2
    2221
    2322btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false),
    … …  
    2625                                        //m_traversalMode(TRAVERSAL_RECURSIVE)
    2726                                        ,m_subtreeHeaderCount(0) //PCK: add this line
    28 {
    29         m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY);
    30         m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
     27{
     28
    3129}
    3230
    … …  
    122120        int curIndex = m_curNodeIndex;
    123121
    124         btAssert(numIndices>0);
     122        assert(numIndices>0);
    125123
    126124        if (numIndices==1)
    … …  
    143141        int internalNodeIndex = m_curNodeIndex;
    144142       
    145         //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value.
    146         //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values
    147         setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
    148         setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
    149        
     143        setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
     144        setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
    150145       
    151146        for (i=startIndex;i<endIndex;i++)
    … …  
    183178                        updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
    184179                }
    185         } else
    186         {
    187 
    188180        }
    189181
    … …  
    347339int maxIterations = 0;
    348340
    349 
    350341void    btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
    351342{
    … …  
    362353        {
    363354                //catch bugs in tree data
    364                 btAssert (walkIterations < m_curNodeIndex);
     355                assert (walkIterations < m_curNodeIndex);
    365356
    366357                walkIterations++;
    … …  
    444435
    445436
    446 void    btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
    447 {
    448         btAssert(!m_useQuantization);
    449 
    450         const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
    451         int escapeIndex, curIndex = 0;
    452         int walkIterations = 0;
    453         bool isLeafNode;
    454         //PCK: unsigned instead of bool
    455         unsigned aabbOverlap=0;
    456         unsigned rayBoxOverlap=0;
    457         btScalar lambda_max = 1.0;
    458        
    459                 /* Quick pruning by quantized box */
    460         btVector3 rayAabbMin = raySource;
    461         btVector3 rayAabbMax = raySource;
    462         rayAabbMin.setMin(rayTarget);
    463         rayAabbMax.setMax(rayTarget);
    464 
    465         /* Add box cast extents to bounding box */
    466         rayAabbMin += aabbMin;
    467         rayAabbMax += aabbMax;
    468 
    469 #ifdef RAYAABB2
    470         btVector3 rayFrom = raySource;
    471         btVector3 rayDir = (rayTarget-raySource);
    472         rayDir.normalize ();
    473         lambda_max = rayDir.dot(rayTarget-raySource);
    474         ///what about division by zero? --> just set rayDirection[i] to 1.0
    475         btVector3 rayDirectionInverse;
    476         rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
    477         rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
    478         rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
    479         unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
    480 #endif
    481 
    482         btVector3 bounds[2];
    483 
    484         while (curIndex < m_curNodeIndex)
    485         {
    486                 btScalar param = 1.0;
    487                 //catch bugs in tree data
    488                 btAssert (walkIterations < m_curNodeIndex);
    489 
    490                 walkIterations++;
    491 
    492                 bounds[0] = rootNode->m_aabbMinOrg;
    493                 bounds[1] = rootNode->m_aabbMaxOrg;
    494                 /* Add box cast extents */
    495                 bounds[0] += aabbMin;
    496                 bounds[1] += aabbMax;
    497 
    498                 aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
    499                 //perhaps profile if it is worth doing the aabbOverlap test first
    500 
    501 #ifdef RAYAABB2
    502                         ///careful with this check: need to check division by zero (above) and fix the unQuantize method
    503                         ///thanks Joerg/hiker for the reproduction case!
    504                         ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
    505                 rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false;
    506 
    507 #else
    508                 btVector3 normal;
    509                 rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal);
    510 #endif
    511 
    512                 isLeafNode = rootNode->m_escapeIndex == -1;
    513                
    514                 //PCK: unsigned instead of bool
    515                 if (isLeafNode && (rayBoxOverlap != 0))
    516                 {
    517                         nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
    518                 }
    519                
    520                 //PCK: unsigned instead of bool
    521                 if ((rayBoxOverlap != 0) || isLeafNode)
    522                 {
    523                         rootNode++;
    524                         curIndex++;
    525                 } else
    526                 {
    527                         escapeIndex = rootNode->m_escapeIndex;
    528                         rootNode += escapeIndex;
    529                         curIndex += escapeIndex;
    530                 }
    531         }
    532         if (maxIterations < walkIterations)
    533                 maxIterations = walkIterations;
    534 
    535 }
    536 
    537437
    538438
    … …  
    555455
    556456        btScalar lambda_max = 1.0;
    557 
     457#define RAYAABB2
    558458#ifdef RAYAABB2
    559459        btVector3 rayFrom = raySource;
    … …  
    603503
    604504                //catch bugs in tree data
    605                 btAssert (walkIterations < subTreeSize);
     505                assert (walkIterations < subTreeSize);
    606506
    607507                walkIterations++;
    … …  
    634534                        ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
    635535
    636                         //BT_PROFILE("btRayAabb2");
    637536                        rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
    638                        
    639537#else
    640538                        rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
    … …  
    700598
    701599                //catch bugs in tree data
    702                 btAssert (walkIterations < subTreeSize);
     600                assert (walkIterations < subTreeSize);
    703601
    704602                walkIterations++;
    … …  
    755653void    btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
    756654{
    757         reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
     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        }
    758667}
    759668
    … …  
    761670void    btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
    762671{
    763         //always use stackless
    764 
    765         if (m_useQuantization)
     672        bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
     673        if (fast_path)
    766674        {
    767675                walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
    768         }
    769         else
    770         {
    771                 walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
    772         }
    773         /*
    774         {
    775                 //recursive traversal
     676        } else {
     677                /* Slow path:
     678                   Construct the bounding box for the entire box cast and send that down the tree */
    776679                btVector3 qaabbMin = raySource;
    777680                btVector3 qaabbMax = raySource;
    … …  
    782685                reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
    783686        }
    784         */
    785 
    786687}
    787688
    … …  
    843744bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
    844745{
    845         btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
     746        assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
    846747        m_subtreeHeaderCount = m_SubtreeHeaders.size();
    847748

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

    r1963 r1972  
    297297        void    walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
    298298        void    walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
    299         void    walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
    300299
    301300        ///tree traversal designed for small-memory processors like PS3 SPU

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

    r1963 r1972  
    8989                return 0; //should never happen, but don't let the game crash ;-)
    9090        }
    91         btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
     91        assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
    9292
    9393        int newHandleIndex = allocHandle();
    … …  
    138138}
    139139
    140 void    btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
    141 {
    142         const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
    143         aabbMin = sbp->m_aabbMin;
    144         aabbMax = sbp->m_aabbMax;
    145 }
    146 
    147140void    btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
    148141{
    149142        btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
    150         sbp->m_aabbMin = aabbMin;
    151         sbp->m_aabbMax = aabbMax;
    152 }
    153 
    154 void    btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
    155 {
    156         for (int i=0;i<m_numHandles;i++)
    157         {
    158                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
    159                 rayCallback.process(proxy);
    160         }
    161 }
     143        sbp->m_min = aabbMin;
     144        sbp->m_max = aabbMax;
     145}
     146
     147
    162148
    163149
    … …  
    169155bool    btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
    170156{
    171         return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
    172                    proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
    173                    proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
     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];
    174160
    175161}

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

    r1963 r1972  
    2323struct btSimpleBroadphaseProxy : public btBroadphaseProxy
    2424{
     25        btVector3       m_min;
     26        btVector3       m_max;
    2527        int                     m_nextFree;
    2628       
    … …  
    3133
    3234        btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
    33         :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
     35        :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy),
     36        m_min(minpt),m_max(maxpt)               
    3437        {
    3538                (void)shapeType;
    … …  
    9396        }
    9497
    95         inline const btSimpleBroadphaseProxy*   getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
    96         {
    97                 const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
    98                 return proxy0;
    99         }
    100 
    10198
    10299        void    validate();
    … …  
    121118        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    122119        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
    123         virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
    124 
    125         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
    126120               
    127121        btOverlappingPairCache* getOverlappingPairCache()
Note: See TracChangeset for help on using the changeset viewer.