Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Changeset 1972


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
Files:
1 deleted
47 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()

  • code/branches/physics/src/bullet/BulletCollision/CMakeLists.txt

    r1963 r1972  
    124124                                CollisionShapes/btUniformScalingShape.cpp
    125125                                CollisionShapes/btUniformScalingShape.h
    126                                 Gimpact/btContactProcessing.cpp
    127                                 Gimpact/btGImpactShape.cpp
    128                                 Gimpact/gim_contact.cpp
    129                                 Gimpact/btGImpactBvh.cpp
    130                                 Gimpact/btGenericPoolAllocator.cpp
    131                                 Gimpact/gim_memory.cpp
    132                                 Gimpact/btGImpactCollisionAlgorithm.cpp
    133                                 Gimpact/btTriangleShapeEx.cpp
    134                                 Gimpact/gim_tri_collision.cpp
    135                                 Gimpact/btGImpactQuantizedBvh.cpp
    136                                 Gimpact/gim_box_set.cpp
    137126                                NarrowPhaseCollision/btContinuousConvexCollision.cpp
    138127                                NarrowPhaseCollision/btContinuousConvexCollision.h

  • code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

    r1963 r1972  
    238238
    239239
    240         SIMD_FORCE_INLINE btBroadphaseProxy*    getBroadphaseHandle()
     240        btBroadphaseProxy*      getBroadphaseHandle()
    241241        {
    242242                return m_broadphaseHandle;
    243243        }
    244244
    245         SIMD_FORCE_INLINE const btBroadphaseProxy*      getBroadphaseHandle() const
     245        const btBroadphaseProxy*        getBroadphaseHandle() const
    246246        {
    247247                return m_broadphaseHandle;

  • code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

    r1963 r1972  
    3333#include "LinearMath/btStackAlloc.h"
    3434
    35 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
    36 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
    37 //#define RECALCULATE_AABB_RAYCAST 1
    3835
    3936//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
    … …  
    230227        if (collisionShape->isConvex())
    231228        {
    232                 BT_PROFILE("rayTestConvex");
    233229                btConvexCast::CastResult castResult;
    234230                castResult.m_fraction = resultCallback.m_closestHitFraction;
    … …  
    274270                if (collisionShape->isConcave())
    275271                {
    276                         BT_PROFILE("rayTestConcave");
    277272                        if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
    278273                        {
    … …  
    380375                        }
    381376                } else {
    382                         BT_PROFILE("rayTestCompound");
    383377                        //todo: use AABB tree or other BVH acceleration structure!
    384378                        if (collisionShape->isCompound())
    … …  
    603597}
    604598
    605 
    606 struct btSingleRayCallback : public btBroadphaseRayCallback
    607 {
    608 
    609         btVector3       m_rayFromWorld;
    610         btVector3       m_rayToWorld;
    611         const btCollisionWorld* m_world;
    612         btCollisionWorld::RayResultCallback&    m_resultCallback;
    613 
    614         btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
    615         :m_rayFromWorld(rayFromWorld),
    616         m_rayToWorld(rayToWorld),
    617         m_world(world),
    618         m_resultCallback(resultCallback)
    619         {
    620 
    621         }
    622 
    623         virtual bool    process(const btBroadphaseProxy* proxy)
     599void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
     600{
     601
     602
     603        btTransform     rayFromTrans,rayToTrans;
     604        rayFromTrans.setIdentity();
     605        rayFromTrans.setOrigin(rayFromWorld);
     606        rayToTrans.setIdentity();
     607
     608        rayToTrans.setOrigin(rayToWorld);
     609
     610        /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
     611
     612        int i;
     613        for (i=0;i<m_collisionObjects.size();i++)
    624614        {
    625615                ///terminate further ray tests, once the closestHitFraction reached zero
    626                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
    627                         return false;
    628 
    629                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
    630 
     616                if (resultCallback.m_closestHitFraction == btScalar(0.f))
     617                        break;
     618
     619                btCollisionObject*      collisionObject= m_collisionObjects[i];
    631620                //only perform raycast if filterMask matches
    632                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
    633                 {
     621                if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
    634622                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
    635                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    636                        
    637 #ifdef RECALCULATE_AABB
    638623                        btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    639624                        collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
    640 #else
    641                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
    642                         btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
    643                         btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
    644 #endif
    645                         btScalar hitLambda = m_resultCallback.m_closestHitFraction;
     625
     626                        btScalar hitLambda = resultCallback.m_closestHitFraction;
    646627                        btVector3 hitNormal;
    647                         if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
    648                         {
    649                                 btTransform     rayFromTrans,rayToTrans;
    650                                 rayFromTrans.setIdentity();
    651                                 rayFromTrans.setOrigin(m_rayFromWorld);
    652                                 rayToTrans.setIdentity();
    653                                 rayToTrans.setOrigin(m_rayToWorld);
    654 
    655                                 m_world->rayTestSingle(rayFromTrans,rayToTrans,
     628                        if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
     629                        {
     630                                rayTestSingle(rayFromTrans,rayToTrans,
    656631                                        collisionObject,
    657632                                                collisionObject->getCollisionShape(),
    658633                                                collisionObject->getWorldTransform(),
    659                                                 m_resultCallback);
    660                         }
    661                 }
    662                 return true;
    663         }
    664 };
    665 
    666 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
    667 {
    668         BT_PROFILE("rayTest");
    669         /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
    670         btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
    671 
    672 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
    673         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
    674 #else
    675         for (int i=0;i<this->getNumCollisionObjects();i++)
    676         {
    677                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
    678         }       
    679 #endif //USE_BRUTEFORCE_RAYBROADPHASE
     634                                                resultCallback);
     635                        }
     636                }
     637
     638        }
    680639
    681640}

  • code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h

    r1963 r1972  
    108108        }
    109109
    110         const btBroadphaseInterface*    getBroadphase() const
    111         {
    112                 return m_broadphasePairCache;
    113         }
    114 
    115110        btBroadphaseInterface*  getBroadphase()
    116111        {

  • code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp

    r1963 r1972  
    1818#include "LinearMath/btQuaternion.h"
    1919
     20btConvexPointCloudShape::btConvexPointCloudShape (btVector3* points,int numPoints) : btPolyhedralConvexShape ()
     21{
     22        m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
     23        m_points = points;
     24        m_numPoints = numPoints;
     25
     26        recalcLocalAabb();
     27}
     28
     29void btConvexPointCloudShape::setPoints (btVector3* points, int numPoints)
     30{
     31        m_points = points;
     32        m_numPoints = numPoints;
     33
     34        recalcLocalAabb();
     35}
     36
     37
    2038void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling)
    2139{
    … …  
    2442}
    2543
    26 #ifndef __SPU__
    2744btVector3       btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
    2845{
    … …  
    108125
    109126
    110 #endif
     127
    111128
    112129

  • code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.h

    r1963 r1972  
    2929        BT_DECLARE_ALIGNED_ALLOCATOR();
    3030
    31         btConvexPointCloudShape(btVector3* points,int numPoints, bool computeAabb = true)
    32         {
    33                 m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
    34                 m_points = points;
    35                 m_numPoints = numPoints;
     31        btConvexPointCloudShape(btVector3* points,int numPoints);
    3632
    37                 if (computeAabb)
    38                         recalcLocalAabb();
    39         }
    40 
    41         void setPoints (btVector3* points, int numPoints, bool computeAabb = true)
    42         {
    43                 m_points = points;
    44                 m_numPoints = numPoints;
    45 
    46                 if (computeAabb)
    47                         recalcLocalAabb();
    48         }
     33        void setPoints (btVector3* points, int numPoints);
    4934
    5035        btPoint3* getPoints()
    … …  
    6348        }
    6449
    65 #ifndef __SPU__
    6650        virtual btVector3       localGetSupportingVertex(const btVector3& vec)const;
    6751        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
    6852        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
    69 #endif
    7053
    7154

  • code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

    r1963 r1972  
    1616#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
    1717
    18 btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
     18btPolyhedralConvexShape::btPolyhedralConvexShape()
     19:btConvexInternalShape(),
    1920m_localAabbMin(1,1,1),
    2021m_localAabbMax(-1,-1,-1),
    … …  
    2425
    2526}
     27
    2628
    2729

  • code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h

    r1963 r1972  
    3232        bool            m_isLocalAabbValid;
    3333
     34        btPolyhedralConvexShape();
    3435public:
    3536
    36         btPolyhedralConvexShape();
     37       
    3738
    3839        //brute force implementations
    39 
    4040        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
    4141        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
    42 
    4342       
    4443        virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
    4544
    46 
    47         void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
    48         {
    49                 m_isLocalAabbValid = true;
    50                 m_localAabbMin = aabbMin;
    51                 m_localAabbMax = aabbMax;
    52         }
    53 
    54         inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
    55         {
    56                 btAssert(m_isLocalAabbValid);
    57                 aabbMin = m_localAabbMin;
    58                 aabbMax = m_localAabbMax;
    59         }
    6045
    6146        inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const

  • code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.cpp

    r1963 r1972  
    1818
    1919#include "LinearMath/btQuaternion.h"
     20
     21
     22btSphereShape ::btSphereShape (btScalar radius) : btConvexInternalShape ()
     23{
     24        m_shapeType = SPHERE_SHAPE_PROXYTYPE;
     25        m_implicitShapeDimensions.setX(radius);
     26        m_collisionMargin = radius;
     27}
    2028
    2129btVector3       btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const

  • code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h

    r1963 r1972  
    2828        BT_DECLARE_ALIGNED_ALLOCATOR();
    2929
    30         btSphereShape (btScalar radius) : btConvexInternalShape ()
    31         {
    32                 m_shapeType = SPHERE_SHAPE_PROXYTYPE;
    33                 m_implicitShapeDimensions.setX(radius);
    34                 m_collisionMargin = radius;
    35         }
     30        btSphereShape (btScalar radius);
     31       
    3632       
    3733        virtual btVector3       localGetSupportingVertex(const btVector3& vec)const;

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

    r1963 r1972  
    886886        const btVector3 delta=  results.witnesses[1]-
    887887                                                        results.witnesses[0];
    888         const btScalar  margin= shape0->getMarginNonVirtual()+
    889                                                         shape1.getMarginNonVirtual();
     888        const btScalar  margin= shape0->getMargin()+
     889                                                        shape1.getMargin();
    890890        const btScalar  length= delta.length();
    891891        results.normal                  =       delta/length;

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

    r1963 r1972  
    2222
    2323#if defined(DEBUG) || defined (_DEBUG)
    24 //#define TEST_NON_VIRTUAL 1
     24#define TEST_NON_VIRTUAL 1
    2525#include <stdio.h> //for debug printf
    2626#ifdef __SPU__

  • code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

    r1963 r1972  
    4545
    4646        int                     m_solverMode;
    47         int     m_restingContactRestitutionThreshold;
    4847
    4948
    … …  
    7170                m_warmstartingFactor=btScalar(0.85);
    7271                m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING;
    73                 m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
    7472        }
    7573};

  • code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

    r1963 r1972  
    719719
    720720                                                        solverConstraint.m_friction = cp.m_combinedFriction;
    721 
    722                                                        
    723                                                         if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
     721                                                        solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
     722                                                        if (solverConstraint.m_restitution <= btScalar(0.))
    724723                                                        {
    725724                                                                solverConstraint.m_restitution = 0.f;
    726                                                         } else
    727                                                         {
    728                                                                 solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
    729                                                                 if (solverConstraint.m_restitution <= btScalar(0.))
    730                                                                 {
    731                                                                         solverConstraint.m_restitution = 0.f;
    732                                                                 };
    733                                                         }
     725                                                        };
    734726
    735727                                                       
    … …  
    994986                pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
    995987                pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    996                 pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     988                pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
    997989
    998990                //do a callback here?
    … …  
    12291221                                cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
    12301222                                cpd->m_friction = cp.m_combinedFriction;
    1231                                 if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
    1232                                 {
    1233                                         cpd->m_restitution = 0.f;
    1234                                 } else
    1235                                 {
    1236                                         cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
    1237                                         if (cpd->m_restitution <= btScalar(0.))
    1238                                         {
    1239                                                 cpd->m_restitution = btScalar(0.0);
    1240                                         };
    1241                                 }
     1223                                cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
     1224                                if (cpd->m_restitution <= btScalar(0.))
     1225                                {
     1226                                        cpd->m_restitution = btScalar(0.0);
     1227
     1228                                };
    12421229                               
    12431230                                //restitution and penetration work in same direction so

  • code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

    r1963 r1972  
    126126void    btDiscreteDynamicsWorld::debugDrawWorld()
    127127{
    128         BT_PROFILE("debugDrawWorld");
    129128
    130129        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
    … …  
    255254void    btDiscreteDynamicsWorld::synchronizeMotionStates()
    256255{
    257         BT_PROFILE("synchronizeMotionStates");
    258256        {
    259257                //todo: iterate over awake simulation islands!

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.cpp

    r1963 r1972  
    151151btSoftBody::Material*           btSoftBody::appendMaterial()
    152152{
    153         Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
    154         if(m_materials.size()>0)
    155                 *pm=*m_materials[0];
     153Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
     154if(m_materials.size()>0)
     155        *pm=*m_materials[0];
    156156        else
    157                 ZeroInitialize(*pm);
    158         m_materials.push_back(pm);
    159         return(pm);
     157        ZeroInitialize(*pm);
     158m_materials.push_back(pm);
     159return(pm);
    160160}
    161161
    162162//
    163163void                    btSoftBody::appendNote( const char* text,
    164                                                                            const btVector3& o,
    165                                                                            const btVector4& c,
    166                                                                            Node* n0,
    167                                                                            Node* n1,
    168                                                                            Node* n2,
    169                                                                            Node* n3)
    170 {
    171         Note    n;
    172         ZeroInitialize(n);
    173         n.m_rank                =       0;
    174         n.m_text                =       text;
    175         n.m_offset              =       o;
    176         n.m_coords[0]   =       c.x();
    177         n.m_coords[1]   =       c.y();
    178         n.m_coords[2]   =       c.z();
    179         n.m_coords[3]   =       c.w();
    180         n.m_nodes[0]    =       n0;n.m_rank+=n0?1:0;
    181         n.m_nodes[1]    =       n1;n.m_rank+=n1?1:0;
    182         n.m_nodes[2]    =       n2;n.m_rank+=n2?1:0;
    183         n.m_nodes[3]    =       n3;n.m_rank+=n3?1:0;
    184         m_notes.push_back(n);
     164                                                                                const btVector3& o,
     165                                                                                const btVector4& c,
     166                                                                                Node* n0,
     167                                                                                Node* n1,
     168                                                                                Node* n2,
     169                                                                                Node* n3)
     170{
     171Note    n;
     172ZeroInitialize(n);
     173n.m_rank                =       0;
     174n.m_text                =       text;
     175n.m_offset              =       o;
     176n.m_coords[0]   =       c.x();
     177n.m_coords[1]   =       c.y();
     178n.m_coords[2]   =       c.z();
     179n.m_coords[3]   =       c.w();
     180n.m_nodes[0]    =       n0;n.m_rank+=n0?1:0;
     181n.m_nodes[1]    =       n1;n.m_rank+=n1?1:0;
     182n.m_nodes[2]    =       n2;n.m_rank+=n2?1:0;
     183n.m_nodes[3]    =       n3;n.m_rank+=n3?1:0;
     184m_notes.push_back(n);
    185185}
    186186
    187187//
    188188void                    btSoftBody::appendNote( const char* text,
    189                                                                            const btVector3& o,
    190                                                                            Node* feature)
    191 {
    192         appendNote(text,o,btVector4(1,0,0,0),feature);
     189                                                                                const btVector3& o,
     190                                                                                Node* feature)
     191{
     192appendNote(text,o,btVector4(1,0,0,0),feature);
    193193}
    194194
    195195//
    196196void                    btSoftBody::appendNote( const char* text,
    197                                                                            const btVector3& o,
    198                                                                            Link* feature)
    199 {
    200         static const btScalar   w=1/(btScalar)2;
    201         appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
    202                 feature->m_n[1]);
    203 }
    204 
     197                                                                                const btVector3& o,
     198                                                                                Link* feature)
     199{
     200static const btScalar   w=1/(btScalar)2;
     201appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
     202                                                                                feature->m_n[1]);
     203}
     204                                                               
    205205//
    206206void                    btSoftBody::appendNote( const char* text,
    207                                                                            const btVector3& o,
    208                                                                            Face* feature)
    209 {
    210         static const btScalar   w=1/(btScalar)3;
    211         appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
    212                 feature->m_n[1],
    213                 feature->m_n[2]);
     207                                                                                const btVector3& o,
     208                                                                                Face* feature)
     209{
     210static const btScalar   w=1/(btScalar)3;
     211appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
     212                                                                                feature->m_n[1],
     213                                                                                feature->m_n[2]);
    214214}
    215215
    … …  
    217217void                    btSoftBody::appendNode( const btVector3& x,btScalar m)
    218218{
    219         if(m_nodes.capacity()==m_nodes.size())
    220         {
    221                 pointersToIndices();
    222                 m_nodes.reserve(m_nodes.size()*2+1);
    223                 indicesToPointers();
    224         }
    225         const btScalar  margin=getCollisionShape()->getMargin();
    226         m_nodes.push_back(Node());
    227         Node&                   n=m_nodes[m_nodes.size()-1];
    228         ZeroInitialize(n);
    229         n.m_x                   =       x;
    230         n.m_q                   =       n.m_x;
    231         n.m_im                  =       m>0?1/m:0;
    232         n.m_material    =       m_materials[0];
    233         n.m_leaf                =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
     219if(m_nodes.capacity()==m_nodes.size())
     220        {
     221        pointersToIndices();
     222        m_nodes.reserve(m_nodes.size()*2+1);
     223        indicesToPointers();
     224        }
     225const btScalar  margin=getCollisionShape()->getMargin();
     226m_nodes.push_back(Node());
     227Node&                   n=m_nodes[m_nodes.size()-1];
     228ZeroInitialize(n);
     229n.m_x                   =       x;
     230n.m_q                   =       n.m_x;
     231n.m_im                  =       m>0?1/m:0;
     232n.m_material    =       m_materials[0];
     233n.m_leaf                =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
    234234}
    235235
    … …  
    237237void                    btSoftBody::appendLink(int model,Material* mat)
    238238{
    239         Link    l;
    240         if(model>=0)
    241                 l=m_links[model];
     239Link    l;
     240if(model>=0)
     241        l=m_links[model];
    242242        else
    243243        { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
    244         m_links.push_back(l);
     244m_links.push_back(l);
    245245}
    246246
    … …  
    274274void                    btSoftBody::appendFace(int model,Material* mat)
    275275{
    276         Face    f;
    277         if(model>=0)
     276Face    f;
     277if(model>=0)
    278278        { f=m_faces[model]; }
    279279        else
    280280        { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
    281         m_faces.push_back(f);
     281m_faces.push_back(f);
    282282}
    283283
    … …  
    320320void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
    321321{
    322         LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
    323         pj->m_bodies[0] =       body0;
    324         pj->m_bodies[1] =       body1;
    325         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
    326         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
    327         pj->m_cfm               =       specs.cfm;
    328         pj->m_erp               =       specs.erp;
    329         pj->m_split             =       specs.split;
    330         m_joints.push_back(pj);
     322LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
     323pj->m_bodies[0] =       body0;
     324pj->m_bodies[1] =       body1;
     325pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
     326pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
     327pj->m_cfm               =       specs.cfm;
     328pj->m_erp               =       specs.erp;
     329pj->m_split             =       specs.split;
     330m_joints.push_back(pj);
    331331}
    332332
    … …  
    334334void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
    335335{
    336         appendLinearJoint(specs,m_clusters[0],body);
     336appendLinearJoint(specs,m_clusters[0],body);
    337337}
    338338
    … …  
    340340void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
    341341{
    342         appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
     342appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
    343343}
    344344
    … …  
    346346void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
    347347{
    348         AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
    349         pj->m_bodies[0] =       body0;
    350         pj->m_bodies[1] =       body1;
    351         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
    352         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
    353         pj->m_cfm               =       specs.cfm;
    354         pj->m_erp               =       specs.erp;
    355         pj->m_split             =       specs.split;
    356         pj->m_icontrol  =       specs.icontrol;
    357         m_joints.push_back(pj);
     348AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
     349pj->m_bodies[0] =       body0;
     350pj->m_bodies[1] =       body1;
     351pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
     352pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
     353pj->m_cfm               =       specs.cfm;
     354pj->m_erp               =       specs.erp;
     355pj->m_split             =       specs.split;
     356pj->m_icontrol  =       specs.icontrol;
     357m_joints.push_back(pj);
    358358}
    359359
    … …  
    361361void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
    362362{
    363         appendAngularJoint(specs,m_clusters[0],body);
     363appendAngularJoint(specs,m_clusters[0],body);
    364364}
    365365
    … …  
    367367void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
    368368{
    369         appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
     369appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
    370370}
    371371
    … …  
    456456                        const Face&             f=m_faces[i];
    457457                        const btScalar  twicearea=AreaOf(       f.m_n[0]->m_x,
    458                                 f.m_n[1]->m_x,
    459                                 f.m_n[2]->m_x);
     458                                                                                                f.m_n[1]->m_x,
     459                                                                                                f.m_n[2]->m_x);
    460460                        for(int j=0;j<3;++j)
    461461                        {
    … …  
    503503void                    btSoftBody::translate(const btVector3& trs)
    504504{
    505         btTransform     t;
    506         t.setIdentity();
    507         t.setOrigin(trs);
    508         transform(t);
     505btTransform     t;
     506t.setIdentity();
     507t.setOrigin(trs);
     508transform(t);
    509509}
    510510
    … …  
    512512void                    btSoftBody::rotate(     const btQuaternion& rot)
    513513{
    514         btTransform     t;
    515         t.setIdentity();
    516         t.setRotation(rot);
    517         transform(t);
     514btTransform     t;
     515t.setIdentity();
     516t.setRotation(rot);
     517transform(t);
    518518}
    519519
    … …  
    554554                Node&   n=m_nodes[i];
    555555                m_pose.m_wgh[i]=        n.m_im>0                                        ?
    556                         1/(m_nodes[i].m_im*tmass)       :
    557                 kmass/tmass;
     556                                                        1/(m_nodes[i].m_im*tmass)       :
     557                                                        kmass/tmass;
    558558        }
    559559        /* Pos          */
    … …  
    570570        /* Aqq          */
    571571        m_pose.m_aqq[0] =
    572                 m_pose.m_aqq[1] =
    573                 m_pose.m_aqq[2] =       btVector3(0,0,0);
     572        m_pose.m_aqq[1] =
     573        m_pose.m_aqq[2] =       btVector3(0,0,0);
    574574        for( i=0,ni=m_nodes.size();i<ni;++i)
    575         {
     575                {
    576576                const btVector3&        q=m_pose.m_pos[i];
    577577                const btVector3         mq=m_pose.m_wgh[i]*q;
    … …  
    579579                m_pose.m_aqq[1]+=mq.y()*q;
    580580                m_pose.m_aqq[2]+=mq.z()*q;
    581         }
     581                }
    582582        m_pose.m_aqq=m_pose.m_aqq.inverse();
    583583        updateConstants();
    … …  
    606606int                             btSoftBody::clusterCount() const
    607607{
    608         return(m_clusters.size());
     608return(m_clusters.size());
    609609}
    610610
    … …  
    612612btVector3               btSoftBody::clusterCom(const Cluster* cluster)
    613613{
    614         btVector3               com(0,0,0);
    615         for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
    616         {
    617                 com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
    618         }
    619         return(com*cluster->m_imass);
     614btVector3               com(0,0,0);
     615for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
     616        {
     617        com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
     618        }
     619return(com*cluster->m_imass);
    620620}
    621621
    … …  
    623623btVector3               btSoftBody::clusterCom(int cluster) const
    624624{
    625         return(clusterCom(m_clusters[cluster]));
     625return(clusterCom(m_clusters[cluster]));
    626626}
    627627
    … …  
    629629btVector3               btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
    630630{
    631         return(cluster->m_lv+cross(cluster->m_av,rpos));
     631return(cluster->m_lv+cross(cluster->m_av,rpos));
    632632}
    633633
    … …  
    635635void                    btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
    636636{
    637         const btVector3 li=cluster->m_imass*impulse;
    638         const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
    639         cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
    640         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
    641         cluster->m_nvimpulses++;
     637const btVector3 li=cluster->m_imass*impulse;
     638const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
     639cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
     640cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
     641cluster->m_nvimpulses++;
    642642}
    643643
    … …  
    645645void                    btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
    646646{
    647         const btVector3 li=cluster->m_imass*impulse;
    648         const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
    649         cluster->m_dimpulses[0]+=li;
    650         cluster->m_dimpulses[1]+=ai;
    651         cluster->m_ndimpulses++;
     647const btVector3 li=cluster->m_imass*impulse;
     648const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
     649cluster->m_dimpulses[0]+=li;
     650cluster->m_dimpulses[1]+=ai;
     651cluster->m_ndimpulses++;
    652652}
    653653
    … …  
    655655void                    btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
    656656{
    657         if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
    658         if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
     657if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
     658if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
    659659}
    660660
    … …  
    662662void                    btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
    663663{
    664         const btVector3 ai=cluster->m_invwi*impulse;
    665         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
    666         cluster->m_nvimpulses++;
     664const btVector3 ai=cluster->m_invwi*impulse;
     665cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
     666cluster->m_nvimpulses++;
    667667}
    668668
    … …  
    670670void                    btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
    671671{
    672         const btVector3 ai=cluster->m_invwi*impulse;
    673         cluster->m_dimpulses[1]+=ai;
    674         cluster->m_ndimpulses++;
     672const btVector3 ai=cluster->m_invwi*impulse;
     673cluster->m_dimpulses[1]+=ai;
     674cluster->m_ndimpulses++;
    675675}
    676676
    … …  
    678678void                    btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
    679679{
    680         if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
    681         if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
     680if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
     681if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
    682682}
    683683
    … …  
    685685void                    btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
    686686{
    687         cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
    688         cluster->m_ndimpulses++;
     687cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
     688cluster->m_ndimpulses++;
    689689}
    690690
    … …  
    754754void                    btSoftBody::randomizeConstraints()
    755755{
    756         unsigned long   seed=243703;
     756unsigned long   seed=243703;
    757757#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
    758         int i,ni;
     758int i,ni;
    759759
    760760        for(i=0,ni=m_links.size();i<ni;++i)
    … …  
    772772void                    btSoftBody::releaseCluster(int index)
    773773{
    774         Cluster*        c=m_clusters[index];
    775         if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
    776         c->~Cluster();
    777         btAlignedFree(c);
    778         m_clusters.remove(c);
     774Cluster*        c=m_clusters[index];
     775if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
     776c->~Cluster();
     777btAlignedFree(c);
     778m_clusters.remove(c);
    779779}
    780780
    … …  
    782782void                    btSoftBody::releaseClusters()
    783783{
    784         while(m_clusters.size()>0) releaseCluster(0);
     784while(m_clusters.size()>0) releaseCluster(0);
    785785}
    786786
    … …  
    788788int                             btSoftBody::generateClusters(int k,int maxiterations)
    789789{
    790         int i;
    791         releaseClusters();
    792         m_clusters.resize(btMin(k,m_nodes.size()));
    793         for(i=0;i<m_clusters.size();++i)
    794         {
    795                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
    796                 m_clusters[i]->m_collide=       true;
    797         }
    798         k=m_clusters.size();
    799         if(k>0)
    800         {
    801                 /* Initialize           */
    802                 btAlignedObjectArray<btVector3> centers;
    803                 btVector3                                               cog(0,0,0);
    804                 int                                                             i;
     790int i;
     791releaseClusters();
     792m_clusters.resize(btMin(k,m_nodes.size()));
     793for(i=0;i<m_clusters.size();++i)
     794        {
     795        m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
     796        m_clusters[i]->m_collide=       true;
     797        }
     798k=m_clusters.size();
     799if(k>0)
     800        {
     801        /* Initialize           */
     802        btAlignedObjectArray<btVector3> centers;
     803        btVector3                                               cog(0,0,0);
     804        int                                                             i;
     805        for(i=0;i<m_nodes.size();++i)
     806                {
     807                cog+=m_nodes[i].m_x;
     808                m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
     809                }
     810        cog/=(btScalar)m_nodes.size();
     811        centers.resize(k,cog);
     812        /* Iterate                      */
     813        const btScalar  slope=16;
     814        bool                    changed;
     815        int                             iterations=0;
     816        do      {
     817                const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
     818                changed=false;
     819                iterations++;   
     820                int i;
     821
     822                for(i=0;i<k;++i)
     823                        {
     824                        btVector3       c(0,0,0);
     825                        for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
     826                                {
     827                                c+=m_clusters[i]->m_nodes[j]->m_x;
     828                                }
     829                        if(m_clusters[i]->m_nodes.size())
     830                                {
     831                                c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
     832                                c                       =       centers[i]+(c-centers[i])*w;
     833                                changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
     834                                centers[i]      =       c;
     835                                m_clusters[i]->m_nodes.resize(0);
     836                                }                       
     837                        }
    805838                for(i=0;i<m_nodes.size();++i)
    806                 {
    807                         cog+=m_nodes[i].m_x;
    808                         m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
    809                 }
    810                 cog/=(btScalar)m_nodes.size();
    811                 centers.resize(k,cog);
    812                 /* Iterate                      */
    813                 const btScalar  slope=16;
    814                 bool                    changed;
    815                 int                             iterations=0;
    816                 do      {
    817                         const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
    818                         changed=false;
    819                         iterations++;   
    820                         int i;
    821 
    822                         for(i=0;i<k;++i)
    823                         {
    824                                 btVector3       c(0,0,0);
    825                                 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
     839                        {
     840                        const btVector3 nx=m_nodes[i].m_x;
     841                        int                             kbest=0;
     842                        btScalar                kdist=ClusterMetric(centers[0],nx);
     843                        for(int j=1;j<k;++j)
    826844                                {
    827                                         c+=m_clusters[i]->m_nodes[j]->m_x;
    828                                 }
    829                                 if(m_clusters[i]->m_nodes.size())
    830                                 {
    831                                         c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
    832                                         c                       =       centers[i]+(c-centers[i])*w;
    833                                         changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
    834                                         centers[i]      =       c;
    835                                         m_clusters[i]->m_nodes.resize(0);
    836                                 }                       
    837                         }
    838                         for(i=0;i<m_nodes.size();++i)
    839                         {
    840                                 const btVector3 nx=m_nodes[i].m_x;
    841                                 int                             kbest=0;
    842                                 btScalar                kdist=ClusterMetric(centers[0],nx);
    843                                 for(int j=1;j<k;++j)
    844                                 {
    845                                         const btScalar  d=ClusterMetric(centers[j],nx);
    846                                         if(d<kdist)
     845                                const btScalar  d=ClusterMetric(centers[j],nx);
     846                                if(d<kdist)
    847847                                        {
    848                                                 kbest=j;
    849                                                 kdist=d;
     848                                        kbest=j;
     849                                        kdist=d;
    850850                                        }
    851851                                }
    852                                 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
     852                        m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
    853853                        }               
    854854                } while(changed&&(iterations<maxiterations));
    855                 /* Merge                */
    856                 btAlignedObjectArray<int>       cids;
    857                 cids.resize(m_nodes.size(),-1);
    858                 for(i=0;i<m_clusters.size();++i)
    859                 {
    860                         for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
    861                         {
    862                                 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
    863                         }
    864                 }
    865                 for(i=0;i<m_faces.size();++i)
    866                 {
    867                         const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
    868                                 int(m_faces[i].m_n[1]-&m_nodes[0]),
    869                                 int(m_faces[i].m_n[2]-&m_nodes[0])};
    870                         for(int j=0;j<3;++j)
    871                         {
    872                                 const int cid=cids[idx[j]];
    873                                 for(int q=1;q<3;++q)
     855        /* Merge                */
     856        btAlignedObjectArray<int>       cids;
     857        cids.resize(m_nodes.size(),-1);
     858        for(i=0;i<m_clusters.size();++i)
     859                {
     860                for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
     861                        {
     862                        cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
     863                        }
     864                }
     865        for(i=0;i<m_faces.size();++i)
     866                {
     867                const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
     868                                                        int(m_faces[i].m_n[1]-&m_nodes[0]),
     869                                                        int(m_faces[i].m_n[2]-&m_nodes[0])};
     870                for(int j=0;j<3;++j)
     871                        {
     872                        const int cid=cids[idx[j]];
     873                        for(int q=1;q<3;++q)
    874874                                {
    875                                         const int kid=idx[(j+q)%3];
    876                                         if(cids[kid]!=cid)
     875                                const int kid=idx[(j+q)%3];
     876                                if(cids[kid]!=cid)
    877877                                        {
    878                                                 if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
     878                                        if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
    879879                                                {
    880                                                         m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
     880                                                m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
    881881                                                }
    882882                                        }
    … …  
    884884                        }
    885885                }
    886                 /* Master               */
    887                 if(m_clusters.size()>1)
    888                 {
    889                         Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
    890                         pmaster->m_collide      =       false;
    891                         pmaster->m_nodes.reserve(m_nodes.size());
    892                         for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
    893                         m_clusters.push_back(pmaster);
    894                         btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
    895                 }
    896                 /* Terminate    */
    897                 for(i=0;i<m_clusters.size();++i)
    898                 {
    899                         if(m_clusters[i]->m_nodes.size()==0)
    900                         {
    901                                 releaseCluster(i--);
    902                         }
    903                 }
    904 
    905                 initializeClusters();
    906                 updateClusters();
    907                 return(m_clusters.size());
    908         }
    909         return(0);
     886        /* Master               */
     887        if(m_clusters.size()>1)
     888                {
     889                Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
     890                pmaster->m_collide      =       false;
     891                pmaster->m_nodes.reserve(m_nodes.size());
     892                for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
     893                m_clusters.push_back(pmaster);
     894                btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
     895                }
     896        /* Terminate    */
     897        for(i=0;i<m_clusters.size();++i)
     898                {
     899                if(m_clusters[i]->m_nodes.size()==0)
     900                        {
     901                        releaseCluster(i--);
     902                        }
     903                }
     904               
     905        initializeClusters();
     906        updateClusters();
     907        return(m_clusters.size());
     908        }
     909return(0);
    910910}
    911911
    … …  
    913913void                    btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
    914914{
    915         const Node*                     nbase = &m_nodes[0];
    916         int                                     ncount = m_nodes.size();
    917         btSymMatrix<int>        edges(ncount,-2);
    918         int                                     newnodes=0;
    919         int i,j,k,ni;
    920 
    921         /* Filter out           */
    922         for(i=0;i<m_links.size();++i)
    923         {
    924                 Link&   l=m_links[i];
    925                 if(l.m_bbending)
    926                 {
    927                         if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
    928                         {
    929                                 btSwap(m_links[i],m_links[m_links.size()-1]);
    930                                 m_links.pop_back();--i;
     915const Node*                     nbase = &m_nodes[0];
     916int                                     ncount = m_nodes.size();
     917btSymMatrix<int>        edges(ncount,-2);
     918int                                     newnodes=0;
     919int i,j,k,ni;
     920
     921/* Filter out           */
     922for(i=0;i<m_links.size();++i)
     923        {
     924        Link&   l=m_links[i];
     925        if(l.m_bbending)
     926                {
     927                if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
     928                        {
     929                        btSwap(m_links[i],m_links[m_links.size()-1]);
     930                        m_links.pop_back();--i;
    931931                        }
    932932                }       
    933933        }
    934         /* Fill edges           */
    935         for(i=0;i<m_links.size();++i)
    936         {
    937                 Link&   l=m_links[i];
    938                 edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
    939         }
    940         for(i=0;i<m_faces.size();++i)
     934/* Fill edges           */
     935for(i=0;i<m_links.size();++i)
     936        {
     937        Link&   l=m_links[i];
     938        edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
     939        }
     940for(i=0;i<m_faces.size();++i)
    941941        {       
    942                 Face&   f=m_faces[i];
    943                 edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
    944                 edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
    945                 edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
    946         }
    947         /* Intersect            */
    948         for(i=0;i<ncount;++i)
    949         {
    950                 for(j=i+1;j<ncount;++j)
    951                 {
    952                         if(edges(i,j)==-1)
    953                         {
    954                                 Node&                   a=m_nodes[i];
    955                                 Node&                   b=m_nodes[j];
    956                                 const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
    957                                 if(t>0)
     942        Face&   f=m_faces[i];
     943        edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
     944        edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
     945        edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
     946        }
     947/* Intersect            */
     948for(i=0;i<ncount;++i)
     949        {
     950        for(j=i+1;j<ncount;++j)
     951                {
     952                if(edges(i,j)==-1)
     953                        {
     954                        Node&                   a=m_nodes[i];
     955                        Node&                   b=m_nodes[j];
     956                        const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
     957                        if(t>0)
    958958                                {
    959                                         const btVector3 x=Lerp(a.m_x,b.m_x,t);
    960                                         const btVector3 v=Lerp(a.m_v,b.m_v,t);
    961                                         btScalar                m=0;
    962                                         if(a.m_im>0)
     959                                const btVector3 x=Lerp(a.m_x,b.m_x,t);
     960                                const btVector3 v=Lerp(a.m_v,b.m_v,t);
     961                                btScalar                m=0;
     962                                if(a.m_im>0)
    963963                                        {
    964                                                 if(b.m_im>0)
     964                                        if(b.m_im>0)
    965965                                                {
    966                                                         const btScalar  ma=1/a.m_im;
    967                                                         const btScalar  mb=1/b.m_im;
    968                                                         const btScalar  mc=Lerp(ma,mb,t);
    969                                                         const btScalar  f=(ma+mb)/(ma+mb+mc);
    970                                                         a.m_im=1/(ma*f);
    971                                                         b.m_im=1/(mb*f);
    972                                                         m=mc*f;
     966                                                const btScalar  ma=1/a.m_im;
     967                                                const btScalar  mb=1/b.m_im;
     968                                                const btScalar  mc=Lerp(ma,mb,t);
     969                                                const btScalar  f=(ma+mb)/(ma+mb+mc);
     970                                                a.m_im=1/(ma*f);
     971                                                b.m_im=1/(mb*f);
     972                                                m=mc*f;
    973973                                                }
    974974                                                else
    … …  
    977977                                        else
    978978                                        {
    979                                                 if(b.m_im>0)
     979                                        if(b.m_im>0)
    980980                                                { b.m_im/=0.5;m=1/b.m_im; }
    981981                                                else
    982                                                         m=0;
     982                                                m=0;
    983983                                        }
    984                                         appendNode(x,m);
    985                                         edges(i,j)=m_nodes.size()-1;
    986                                         m_nodes[edges(i,j)].m_v=v;
    987                                         ++newnodes;
     984                                appendNode(x,m);
     985                                edges(i,j)=m_nodes.size()-1;
     986                                m_nodes[edges(i,j)].m_v=v;
     987                                ++newnodes;
    988988                                }
    989989                        }
    990990                }
    991991        }
     992nbase=&m_nodes[0];
     993/* Refine links         */
     994for(i=0,ni=m_links.size();i<ni;++i)
     995        {
     996        Link&           feat=m_links[i];
     997        const int       idx[]={ int(feat.m_n[0]-nbase),
     998                                                int(feat.m_n[1]-nbase)};
     999        if((idx[0]<ncount)&&(idx[1]<ncount))
     1000                {
     1001                const int ni=edges(idx[0],idx[1]);
     1002                if(ni>0)
     1003                        {
     1004                        appendLink(i);
     1005                        Link*           pft[]={ &m_links[i],
     1006                                                                &m_links[m_links.size()-1]};                   
     1007                        pft[0]->m_n[0]=&m_nodes[idx[0]];
     1008                        pft[0]->m_n[1]=&m_nodes[ni];
     1009                        pft[1]->m_n[0]=&m_nodes[ni];
     1010                        pft[1]->m_n[1]=&m_nodes[idx[1]];
     1011                        }
     1012                }
     1013        }
     1014/* Refine faces         */
     1015for(i=0;i<m_faces.size();++i)
     1016        {
     1017        const Face&     feat=m_faces[i];
     1018        const int       idx[]={ int(feat.m_n[0]-nbase),
     1019                                                int(feat.m_n[1]-nbase),
     1020                                                int(feat.m_n[2]-nbase)};
     1021        for(j=2,k=0;k<3;j=k++)
     1022                {
     1023                if((idx[j]<ncount)&&(idx[k]<ncount))
     1024                        {
     1025                        const int ni=edges(idx[j],idx[k]);
     1026                        if(ni>0)
     1027                                {
     1028                                appendFace(i);
     1029                                const int       l=(k+1)%3;
     1030                                Face*           pft[]={ &m_faces[i],
     1031                                                                        &m_faces[m_faces.size()-1]};
     1032                                pft[0]->m_n[0]=&m_nodes[idx[l]];
     1033                                pft[0]->m_n[1]=&m_nodes[idx[j]];
     1034                                pft[0]->m_n[2]=&m_nodes[ni];
     1035                                pft[1]->m_n[0]=&m_nodes[ni];
     1036                                pft[1]->m_n[1]=&m_nodes[idx[k]];
     1037                                pft[1]->m_n[2]=&m_nodes[idx[l]];
     1038                                appendLink(ni,idx[l],pft[0]->m_material);
     1039                                --i;break;
     1040                                }
     1041                        }
     1042                }
     1043        }
     1044/* Cut                          */
     1045if(cut)
     1046        {       
     1047        btAlignedObjectArray<int>       cnodes;
     1048        const int                                       pcount=ncount;
     1049        int                                                     i;
     1050        ncount=m_nodes.size();
     1051        cnodes.resize(ncount,0);
     1052        /* Nodes                */
     1053        for(i=0;i<ncount;++i)
     1054                {
     1055                const btVector3 x=m_nodes[i].m_x;
     1056                if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
     1057                        {
     1058                        const btVector3 v=m_nodes[i].m_v;
     1059                        btScalar                m=getMass(i);
     1060                        if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
     1061                        appendNode(x,m);
     1062                        cnodes[i]=m_nodes.size()-1;
     1063                        m_nodes[cnodes[i]].m_v=v;
     1064                        }
     1065                }
    9921066        nbase=&m_nodes[0];
    993         /* Refine links         */
     1067        /* Links                */
    9941068        for(i=0,ni=m_links.size();i<ni;++i)
    995         {
    996                 Link&           feat=m_links[i];
    997                 const int       idx[]={ int(feat.m_n[0]-nbase),
    998                         int(feat.m_n[1]-nbase)};
    999                 if((idx[0]<ncount)&&(idx[1]<ncount))
    1000                 {
    1001                         const int ni=edges(idx[0],idx[1]);
    1002                         if(ni>0)
    1003                         {
    1004                                 appendLink(i);
    1005                                 Link*           pft[]={ &m_links[i],
    1006                                         &m_links[m_links.size()-1]};                   
    1007                                 pft[0]->m_n[0]=&m_nodes[idx[0]];
    1008                                 pft[0]->m_n[1]=&m_nodes[ni];
    1009                                 pft[1]->m_n[0]=&m_nodes[ni];
    1010                                 pft[1]->m_n[1]=&m_nodes[idx[1]];
    1011                         }
    1012                 }
    1013         }
    1014         /* Refine faces         */
    1015         for(i=0;i<m_faces.size();++i)
    1016         {
    1017                 const Face&     feat=m_faces[i];
    1018                 const int       idx[]={ int(feat.m_n[0]-nbase),
    1019                         int(feat.m_n[1]-nbase),
    1020                         int(feat.m_n[2]-nbase)};
    1021                 for(j=2,k=0;k<3;j=k++)
    1022                 {
    1023                         if((idx[j]<ncount)&&(idx[k]<ncount))
    1024                         {
    1025                                 const int ni=edges(idx[j],idx[k]);
    1026                                 if(ni>0)
     1069                {
     1070                const int               id[]={  int(m_links[i].m_n[0]-nbase),
     1071                                                                int(m_links[i].m_n[1]-nbase)};
     1072                int                             todetach=0;
     1073                if(cnodes[id[0]]&&cnodes[id[1]])
     1074                        {
     1075                        appendLink(i);
     1076                        todetach=m_links.size()-1;
     1077                        }
     1078                else
     1079                        {
     1080                        if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
     1081                                        (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
     1082                                todetach=i;
     1083                        }
     1084                if(todetach)
     1085                        {
     1086                        Link&   l=m_links[todetach];
     1087                        for(int j=0;j<2;++j)
    10271088                                {
    1028                                         appendFace(i);
    1029                                         const int       l=(k+1)%3;
    1030                                         Face*           pft[]={ &m_faces[i],
    1031                                                 &m_faces[m_faces.size()-1]};
    1032                                         pft[0]->m_n[0]=&m_nodes[idx[l]];
    1033                                         pft[0]->m_n[1]=&m_nodes[idx[j]];
    1034                                         pft[0]->m_n[2]=&m_nodes[ni];
    1035                                         pft[1]->m_n[0]=&m_nodes[ni];
    1036                                         pft[1]->m_n[1]=&m_nodes[idx[k]];
    1037                                         pft[1]->m_n[2]=&m_nodes[idx[l]];
    1038                                         appendLink(ni,idx[l],pft[0]->m_material);
    1039                                         --i;break;
     1089                                int cn=cnodes[int(l.m_n[j]-nbase)];
     1090                                if(cn) l.m_n[j]=&m_nodes[cn];
     1091                                }                       
     1092                        }
     1093                }
     1094        /* Faces                */
     1095        for(i=0,ni=m_faces.size();i<ni;++i)
     1096                {
     1097                Node**                  n=      m_faces[i].m_n;
     1098                if(     (ifn->Eval(n[0]->m_x)<accurary)&&
     1099                        (ifn->Eval(n[1]->m_x)<accurary)&&
     1100                        (ifn->Eval(n[2]->m_x)<accurary))
     1101                        {
     1102                        for(int j=0;j<3;++j)
     1103                                {
     1104                                int cn=cnodes[int(n[j]-nbase)];
     1105                                if(cn) n[j]=&m_nodes[cn];
    10401106                                }
    10411107                        }
    10421108                }
    1043         }
    1044         /* Cut                          */
    1045         if(cut)
    1046         {       
    1047                 btAlignedObjectArray<int>       cnodes;
    1048                 const int                                       pcount=ncount;
    1049                 int                                                     i;
    1050                 ncount=m_nodes.size();
    1051                 cnodes.resize(ncount,0);
    1052                 /* Nodes                */
    1053                 for(i=0;i<ncount;++i)
    1054                 {
    1055                         const btVector3 x=m_nodes[i].m_x;
    1056                         if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
    1057                         {
    1058                                 const btVector3 v=m_nodes[i].m_v;
    1059                                 btScalar                m=getMass(i);
    1060                                 if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
    1061                                 appendNode(x,m);
    1062                                 cnodes[i]=m_nodes.size()-1;
    1063                                 m_nodes[cnodes[i]].m_v=v;
    1064                         }
    1065                 }
    1066                 nbase=&m_nodes[0];
    1067                 /* Links                */
    1068                 for(i=0,ni=m_links.size();i<ni;++i)
    1069                 {
    1070                         const int               id[]={  int(m_links[i].m_n[0]-nbase),
    1071                                 int(m_links[i].m_n[1]-nbase)};
    1072                         int                             todetach=0;
    1073                         if(cnodes[id[0]]&&cnodes[id[1]])
    1074                         {
    1075                                 appendLink(i);
    1076                                 todetach=m_links.size()-1;
    1077                         }
    1078                         else
    1079                         {
    1080                                 if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
    1081                                         (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
    1082                                         todetach=i;
    1083                         }
    1084                         if(todetach)
    1085                         {
    1086                                 Link&   l=m_links[todetach];
    1087                                 for(int j=0;j<2;++j)
    1088                                 {
    1089                                         int cn=cnodes[int(l.m_n[j]-nbase)];
    1090                                         if(cn) l.m_n[j]=&m_nodes[cn];
    1091                                 }                       
    1092                         }
    1093                 }
    1094                 /* Faces                */
    1095                 for(i=0,ni=m_faces.size();i<ni;++i)
    1096                 {
    1097                         Node**                  n=      m_faces[i].m_n;
    1098                         if(     (ifn->Eval(n[0]->m_x)<accurary)&&
    1099                                 (ifn->Eval(n[1]->m_x)<accurary)&&
    1100                                 (ifn->Eval(n[2]->m_x)<accurary))
    1101                         {
    1102                                 for(int j=0;j<3;++j)
    1103                                 {
    1104                                         int cn=cnodes[int(n[j]-nbase)];
    1105                                         if(cn) n[j]=&m_nodes[cn];
    1106                                 }
    1107                         }
    1108                 }
    1109                 /* Clean orphans        */
    1110                 int                                                     nnodes=m_nodes.size();
    1111                 btAlignedObjectArray<int>       ranks;
    1112                 btAlignedObjectArray<int>       todelete;
    1113                 ranks.resize(nnodes,0);
    1114                 for(i=0,ni=m_links.size();i<ni;++i)
    1115                 {
    1116                         for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
    1117                 }
    1118                 for(i=0,ni=m_faces.size();i<ni;++i)
    1119                 {
    1120                         for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
    1121                 }
    1122                 for(i=0;i<m_links.size();++i)
    1123                 {
    1124                         const int       id[]={  int(m_links[i].m_n[0]-nbase),
    1125                                 int(m_links[i].m_n[1]-nbase)};
    1126                         const bool      sg[]={  ranks[id[0]]==1,
    1127                                 ranks[id[1]]==1};
    1128                         if(sg[0]||sg[1])
    1129                         {
    1130                                 --ranks[id[0]];
    1131                                 --ranks[id[1]];
    1132                                 btSwap(m_links[i],m_links[m_links.size()-1]);
    1133                                 m_links.pop_back();--i;
    1134                         }
    1135                 }
    1136 #if 0   
    1137                 for(i=nnodes-1;i>=0;--i)
    1138                 {
    1139                         if(!ranks[i]) todelete.push_back(i);
     1109        /* Clean orphans        */
     1110        int                                                     nnodes=m_nodes.size();
     1111        btAlignedObjectArray<int>       ranks;
     1112        btAlignedObjectArray<int>       todelete;
     1113        ranks.resize(nnodes,0);
     1114        for(i=0,ni=m_links.size();i<ni;++i)
     1115                {
     1116                for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
     1117                }
     1118        for(i=0,ni=m_faces.size();i<ni;++i)
     1119                {
     1120                for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
     1121                }
     1122        for(i=0;i<m_links.size();++i)
     1123                {
     1124                const int       id[]={  int(m_links[i].m_n[0]-nbase),
     1125                                                        int(m_links[i].m_n[1]-nbase)};
     1126                const bool      sg[]={  ranks[id[0]]==1,
     1127                                                        ranks[id[1]]==1};
     1128                if(sg[0]||sg[1])
     1129                        {
     1130                        --ranks[id[0]];
     1131                        --ranks[id[1]];
     1132                        btSwap(m_links[i],m_links[m_links.size()-1]);
     1133                        m_links.pop_back();--i;
     1134                        }
     1135                }
     1136        #if 0   
     1137        for(i=nnodes-1;i>=0;--i)
     1138                {
     1139                if(!ranks[i]) todelete.push_back(i);
    11401140                }       
    1141                 if(todelete.size())
     1141        if(todelete.size())
    11421142                {               
    1143                         btAlignedObjectArray<int>&      map=ranks;
    1144                         for(int i=0;i<nnodes;++i) map[i]=i;
    1145                         PointersToIndices(this);
    1146                         for(int i=0,ni=todelete.size();i<ni;++i)
    1147                         {
    1148                                 int             j=todelete[i];
    1149                                 int&    a=map[j];
    1150                                 int&    b=map[--nnodes];
    1151                                 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
    1152                                 btSwap(m_nodes[a],m_nodes[b]);
    1153                                 j=a;a=b;b=j;                   
    1154                         }
    1155                         IndicesToPointers(this,&map[0]);
    1156                         m_nodes.resize(nnodes);
    1157                 }
    1158 #endif
    1159         }
    1160         m_bUpdateRtCst=true;
     1143                btAlignedObjectArray<int>&      map=ranks;
     1144                for(int i=0;i<nnodes;++i) map[i]=i;
     1145                PointersToIndices(this);
     1146                for(int i=0,ni=todelete.size();i<ni;++i)
     1147                        {
     1148                        int             j=todelete[i];
     1149                        int&    a=map[j];
     1150                        int&    b=map[--nnodes];
     1151                        m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
     1152                        btSwap(m_nodes[a],m_nodes[b]);
     1153                        j=a;a=b;b=j;                   
     1154                        }
     1155                IndicesToPointers(this,&map[0]);
     1156                m_nodes.resize(nnodes);
     1157                }
     1158        #endif
     1159        }
     1160m_bUpdateRtCst=true;
    11611161}
    11621162
    … …  
    11641164bool                    btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
    11651165{
    1166         return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
     1166return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
    11671167}
    11681168
    … …  
    11701170bool                    btSoftBody::cutLink(int node0,int node1,btScalar position)
    11711171{
    1172         bool                    done=false;
    1173         int i,ni;
    1174         const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
    1175         const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
    1176         const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
    1177         const btScalar  m=1;
    1178         appendNode(x,m);
    1179         appendNode(x,m);
    1180         Node*                   pa=&m_nodes[node0];
    1181         Node*                   pb=&m_nodes[node1];
    1182         Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
    1183                 &m_nodes[m_nodes.size()-1]};
    1184         pn[0]->m_v=v;
    1185         pn[1]->m_v=v;
    1186         for(i=0,ni=m_links.size();i<ni;++i)
    1187         {
    1188                 const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
     1172bool                    done=false;
     1173int i,ni;
     1174const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
     1175const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
     1176const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
     1177const btScalar  m=1;
     1178appendNode(x,m);
     1179appendNode(x,m);
     1180Node*                   pa=&m_nodes[node0];
     1181Node*                   pb=&m_nodes[node1];
     1182Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
     1183                                                &m_nodes[m_nodes.size()-1]};
     1184pn[0]->m_v=v;
     1185pn[1]->m_v=v;
     1186for(i=0,ni=m_links.size();i<ni;++i)
     1187        {
     1188        const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
     1189        if(mtch!=-1)
     1190                {
     1191                appendLink(i);
     1192                Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
     1193                pft[0]->m_n[1]=pn[mtch];
     1194                pft[1]->m_n[0]=pn[1-mtch];
     1195                done=true;
     1196                }
     1197        }
     1198for(i=0,ni=m_faces.size();i<ni;++i)
     1199        {
     1200        for(int k=2,l=0;l<3;k=l++)
     1201                {
     1202                const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
    11891203                if(mtch!=-1)
    1190                 {
    1191                         appendLink(i);
    1192                         Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
    1193                         pft[0]->m_n[1]=pn[mtch];
    1194                         pft[1]->m_n[0]=pn[1-mtch];
    1195                         done=true;
    1196                 }
    1197         }
    1198         for(i=0,ni=m_faces.size();i<ni;++i)
    1199         {
    1200                 for(int k=2,l=0;l<3;k=l++)
    1201                 {
    1202                         const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
    1203                         if(mtch!=-1)
    1204                         {
    1205                                 appendFace(i);
    1206                                 Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
    1207                                 pft[0]->m_n[l]=pn[mtch];
    1208                                 pft[1]->m_n[k]=pn[1-mtch];
    1209                                 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
    1210                                 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
    1211                         }
    1212                 }
    1213         }
    1214         if(!done)
    1215         {
    1216                 m_ndbvt.remove(pn[0]->m_leaf);
    1217                 m_ndbvt.remove(pn[1]->m_leaf);
    1218                 m_nodes.pop_back();
    1219                 m_nodes.pop_back();
    1220         }
    1221         return(done);
    1222 }
    1223 
    1224 //
    1225 bool                    btSoftBody::rayTest(const btVector3& rayFrom,
    1226                                                                         const btVector3& rayTo,
    1227                                                                         sRayCast& results)
    1228 {
    1229         if(m_faces.size()&&m_fdbvt.empty())
    1230                 initializeFaceTree();
    1231 
    1232         results.body    =       this;
    1233         results.fraction = 1.f;
    1234         results.feature =       eFeature::None;
    1235         results.index   =       -1;
    1236 
    1237         return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
     1204                        {
     1205                        appendFace(i);
     1206                        Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
     1207                        pft[0]->m_n[l]=pn[mtch];
     1208                        pft[1]->m_n[k]=pn[1-mtch];
     1209                        appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
     1210                        appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
     1211                        }
     1212                }
     1213        }
     1214if(!done)
     1215        {
     1216        m_ndbvt.remove(pn[0]->m_leaf);
     1217        m_ndbvt.remove(pn[1]->m_leaf);
     1218        m_nodes.pop_back();
     1219        m_nodes.pop_back();
     1220        }
     1221return(done);
     1222}
     1223
     1224//
     1225bool                    btSoftBody::rayCast(const btVector3& org,
     1226                                                                        const btVector3& dir,
     1227                                                                        sRayCast& results,
     1228                                                                        btScalar maxtime)
     1229{
     1230if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree();
     1231results.body    =       this;
     1232results.time    =       maxtime;
     1233results.feature =       eFeature::None;
     1234results.index   =       -1;
     1235return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0);
    12381236}
    12391237
    … …  
    12411239void                    btSoftBody::setSolver(eSolverPresets::_ preset)
    12421240{
    1243         m_cfg.m_vsequence.clear();
    1244         m_cfg.m_psequence.clear();
    1245         m_cfg.m_dsequence.clear();
    1246         switch(preset)
     1241m_cfg.m_vsequence.clear();
     1242m_cfg.m_psequence.clear();
     1243m_cfg.m_dsequence.clear();
     1244switch(preset)
    12471245        {
    12481246        case    eSolverPresets::Positions:
    1249                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
    1250                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
    1251                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
    1252                 m_cfg.m_psequence.push_back(ePSolver::Linear); 
    1253                 break; 
     1247        m_cfg.m_psequence.push_back(ePSolver::Anchors);
     1248        m_cfg.m_psequence.push_back(ePSolver::RContacts);
     1249        m_cfg.m_psequence.push_back(ePSolver::SContacts);
     1250        m_cfg.m_psequence.push_back(ePSolver::Linear); 
     1251        break; 
    12541252        case    eSolverPresets::Velocities:
    1255                 m_cfg.m_vsequence.push_back(eVSolver::Linear);
    1256 
    1257                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
    1258                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
    1259                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
    1260 
    1261                 m_cfg.m_dsequence.push_back(ePSolver::Linear);
    1262                 break;
     1253        m_cfg.m_vsequence.push_back(eVSolver::Linear);
     1254       
     1255        m_cfg.m_psequence.push_back(ePSolver::Anchors);
     1256        m_cfg.m_psequence.push_back(ePSolver::RContacts);
     1257        m_cfg.m_psequence.push_back(ePSolver::SContacts);
     1258       
     1259        m_cfg.m_dsequence.push_back(ePSolver::Linear);
     1260        break;
    12631261        }
    12641262}
    … …  
    12761274                m_fdbvt.clear();
    12771275                if(m_cfg.collisions&fCollision::VF_SS)
    1278                 {
     1276                        {
    12791277                        initializeFaceTree();                   
    1280                 }
    1281         }
    1282 
     1278                        }
     1279        }
     1280               
    12831281        /* Prepare                              */
    12841282        m_sst.sdt               =       dt*m_cfg.timescale;
    … …  
    13081306                Node&   n=m_nodes[i];
    13091307                m_ndbvt.update( n.m_leaf,
    1310                         btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
    1311                         n.m_v*m_sst.velmrg,
    1312                         m_sst.updmrg);
     1308                                                btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
     1309                                                n.m_v*m_sst.velmrg,
     1310                                                m_sst.updmrg);
    13131311        }
    13141312        /* Faces                                */
    13151313        if(!m_fdbvt.empty())
    1316         {
     1314                {
    13171315                for(int i=0;i<m_faces.size();++i)
    1318                 {
     1316                        {
    13191317                        Face&                   f=m_faces[i];
    13201318                        const btVector3 v=(     f.m_n[0]->m_v+
    1321                                 f.m_n[1]->m_v+
    1322                                 f.m_n[2]->m_v)/3;
     1319                                                                f.m_n[1]->m_v+
     1320                                                                f.m_n[2]->m_v)/3;
    13231321                        m_fdbvt.update( f.m_leaf,
    1324                                 VolumeOf(f,m_sst.radmrg),
    1325                                 v*m_sst.velmrg,
    1326                                 m_sst.updmrg);
    1327                 }
    1328         }
     1322                                                        VolumeOf(f,m_sst.radmrg),
     1323                                                        v*m_sst.velmrg,
     1324                                                        m_sst.updmrg);
     1325                        }
     1326                }
    13291327        /* Pose                                 */
    13301328        updatePose();
    13311329        /* Match                                */
    13321330        if(m_pose.m_bframe&&(m_cfg.kMT>0))
    1333         {
     1331                {
    13341332                const btMatrix3x3       posetrs=m_pose.m_rot;
    13351333                for(int i=0,ni=m_nodes.size();i<ni;++i)
    1336                 {
     1334                        {
    13371335                        Node&   n=m_nodes[i];
    13381336                        if(n.m_im>0)
    1339                         {
     1337                                {
    13401338                                const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
    13411339                                n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
    1342                         }
    1343                 }
    1344         }
     1340                                }
     1341                        }
     1342                }
    13451343        /* Clear contacts               */
    13461344        m_rcontacts.resize(0);
    … …  
    13551353void                    btSoftBody::solveConstraints()
    13561354{
    1357         /* Apply clusters               */
    1358         applyClusters(false);
    1359         /* Prepare links                */
    1360 
    1361         int i,ni;
    1362 
    1363         for(i=0,ni=m_links.size();i<ni;++i)
    1364         {
    1365                 Link&   l=m_links[i];
    1366                 l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
    1367                 l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
    1368         }
    1369         /* Prepare anchors              */
    1370         for(i=0,ni=m_anchors.size();i<ni;++i)
    1371         {
    1372                 Anchor&                 a=m_anchors[i];
    1373                 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
    1374                 a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
    1375                         a.m_node->m_im,
    1376                         a.m_body->getInvMass(),
    1377                         a.m_body->getInvInertiaTensorWorld(),
    1378                         ra);
    1379                 a.m_c1  =       ra;
    1380                 a.m_c2  =       m_sst.sdt*a.m_node->m_im;
    1381                 a.m_body->activate();
    1382         }
    1383         /* Solve velocities             */
    1384         if(m_cfg.viterations>0)
    1385         {
    1386                 /* Solve                        */
    1387                 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
    1388                 {
    1389                         for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
    1390                         {
    1391                                 getSolver(m_cfg.m_vsequence[iseq])(this,1);
    1392                         }
    1393                 }
    1394                 /* Update                       */
    1395                 for(i=0,ni=m_nodes.size();i<ni;++i)
    1396                 {
    1397                         Node&   n=m_nodes[i];
    1398                         n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
    1399                 }
    1400         }
    1401         /* Solve positions              */
    1402         if(m_cfg.piterations>0)
    1403         {
    1404                 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
    1405                 {
    1406                         const btScalar ti=isolve/(btScalar)m_cfg.piterations;
    1407                         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
    1408                         {
    1409                                 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
    1410                         }
    1411                 }
    1412                 const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
    1413                 for(i=0,ni=m_nodes.size();i<ni;++i)
    1414                 {
    1415                         Node&   n=m_nodes[i];
    1416                         n.m_v   =       (n.m_x-n.m_q)*vc;
    1417                         n.m_f   =       btVector3(0,0,0);               
    1418                 }
    1419         }
    1420         /* Solve drift                  */
    1421         if(m_cfg.diterations>0)
    1422         {
    1423                 const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
    1424                 for(i=0,ni=m_nodes.size();i<ni;++i)
    1425                 {
    1426                         Node&   n=m_nodes[i];
    1427                         n.m_q   =       n.m_x;
    1428                 }
    1429                 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
    1430                 {
    1431                         for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
    1432                         {
    1433                                 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
    1434                         }
    1435                 }
    1436                 for(int i=0,ni=m_nodes.size();i<ni;++i)
    1437                 {
    1438                         Node&   n=m_nodes[i];
    1439                         n.m_v   +=      (n.m_x-n.m_q)*vcf;
    1440                 }
    1441         }
    1442         /* Apply clusters               */
    1443         dampClusters();
    1444         applyClusters(true);
     1355/* Apply clusters               */
     1356applyClusters(false);
     1357/* Prepare links                */
     1358
     1359int i,ni;
     1360
     1361for(i=0,ni=m_links.size();i<ni;++i)
     1362        {
     1363        Link&   l=m_links[i];
     1364        l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
     1365        l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
     1366        }
     1367/* Prepare anchors              */
     1368for(i=0,ni=m_anchors.size();i<ni;++i)
     1369        {
     1370        Anchor&                 a=m_anchors[i];
     1371        const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
     1372        a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
     1373                                                                a.m_node->m_im,
     1374                                                                a.m_body->getInvMass(),
     1375                                                                a.m_body->getInvInertiaTensorWorld(),
     1376                                                                ra);
     1377        a.m_c1  =       ra;
     1378        a.m_c2  =       m_sst.sdt*a.m_node->m_im;
     1379        a.m_body->activate();
     1380        }
     1381/* Solve velocities             */
     1382if(m_cfg.viterations>0)
     1383        {
     1384        /* Solve                        */
     1385        for(int isolve=0;isolve<m_cfg.viterations;++isolve)
     1386                {
     1387                for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
     1388                        {
     1389                        getSolver(m_cfg.m_vsequence[iseq])(this,1);
     1390                        }
     1391                }
     1392        /* Update                       */
     1393        for(i=0,ni=m_nodes.size();i<ni;++i)
     1394                {
     1395                Node&   n=m_nodes[i];
     1396                n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
     1397                }
     1398        }
     1399/* Solve positions              */
     1400if(m_cfg.piterations>0)
     1401        {
     1402        for(int isolve=0;isolve<m_cfg.piterations;++isolve)
     1403                {
     1404                const btScalar ti=isolve/(btScalar)m_cfg.piterations;
     1405                for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
     1406                        {
     1407                        getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
     1408                        }
     1409                }
     1410        const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
     1411        for(i=0,ni=m_nodes.size();i<ni;++i)
     1412                {
     1413                Node&   n=m_nodes[i];
     1414                n.m_v   =       (n.m_x-n.m_q)*vc;
     1415                n.m_f   =       btVector3(0,0,0);               
     1416                }
     1417        }
     1418/* Solve drift                  */
     1419if(m_cfg.diterations>0)
     1420        {
     1421        const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
     1422        for(i=0,ni=m_nodes.size();i<ni;++i)
     1423                {
     1424                Node&   n=m_nodes[i];
     1425                n.m_q   =       n.m_x;
     1426                }
     1427        for(int idrift=0;idrift<m_cfg.diterations;++idrift)
     1428                {
     1429                for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
     1430                        {
     1431                        getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
     1432                        }
     1433                }
     1434        for(int i=0,ni=m_nodes.size();i<ni;++i)
     1435                {
     1436                Node&   n=m_nodes[i];
     1437                n.m_v   +=      (n.m_x-n.m_q)*vcf;
     1438                }
     1439        }
     1440/* Apply clusters               */
     1441dampClusters();
     1442applyClusters(true);
    14451443}
    14461444
    … …  
    14481446void                    btSoftBody::staticSolve(int iterations)
    14491447{
    1450         for(int isolve=0;isolve<iterations;++isolve)
    1451         {
    1452                 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
    1453                 {
    1454                         getSolver(m_cfg.m_psequence[iseq])(this,1,0);
     1448for(int isolve=0;isolve<iterations;++isolve)
     1449        {
     1450        for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
     1451                {
     1452                getSolver(m_cfg.m_psequence[iseq])(this,1,0);
    14551453                }
    14561454        }
    … …  
    14601458void                    btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
    14611459{
    1462         /// placeholder
     1460/// placeholder
    14631461}
    14641462
    … …  
    14661464void                    btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
    14671465{
    1468         const int       nb=bodies.size();
    1469         int                     iterations=0;
    1470         int i;
    1471 
    1472         for(i=0;i<nb;++i)
    1473         {
    1474                 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
    1475         }
    1476         for(i=0;i<nb;++i)
    1477         {
    1478                 bodies[i]->prepareClusters(iterations);
    1479         }
    1480         for(i=0;i<iterations;++i)
    1481         {
    1482                 const btScalar sor=1;
    1483                 for(int j=0;j<nb;++j)
    1484                 {
    1485                         bodies[j]->solveClusters(sor);
    1486                 }
    1487         }
    1488         for(i=0;i<nb;++i)
    1489         {
    1490                 bodies[i]->cleanupClusters();
     1466const int       nb=bodies.size();
     1467int                     iterations=0;
     1468int i;
     1469
     1470for(i=0;i<nb;++i)
     1471        {
     1472        iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
     1473        }
     1474for(i=0;i<nb;++i)
     1475        {
     1476        bodies[i]->prepareClusters(iterations);
     1477        }
     1478for(i=0;i<iterations;++i)
     1479        {
     1480        const btScalar sor=1;
     1481        for(int j=0;j<nb;++j)
     1482                {
     1483                bodies[j]->solveClusters(sor);
     1484                }
     1485        }
     1486for(i=0;i<nb;++i)
     1487        {
     1488        bodies[i]->cleanupClusters();
    14911489        }
    14921490}
    … …  
    15001498
    15011499//
    1502 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
    1503 {
    1504         m_rayFrom = rayFrom;
    1505         m_rayNormalizedDirection = (rayTo-rayFrom);
    1506         m_rayTo = rayTo;
    1507         m_mint  =       mxt;
    1508         m_face  =       0;
    1509         m_tests =       0;
    1510 }
    1511 
    1512 //
    1513 void                            btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
    1514 {
    1515         btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
    1516         const btScalar          t=rayFromToTriangle(    m_rayFrom,m_rayTo,m_rayNormalizedDirection,
    1517                 f.m_n[0]->m_x,
    1518                 f.m_n[1]->m_x,
    1519                 f.m_n[2]->m_x,
    1520                 m_mint);
    1521         if((t>0)&&(t<m_mint))
    1522         {
    1523                 m_mint=t;m_face=&f;
    1524         }
    1525         ++m_tests;
    1526 }
    1527 
    1528 //
    1529 btScalar                        btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
    1530                                                                                                                                    const btVector3& rayTo,
    1531                                                                                                                                    const btVector3& rayNormalizedDirection,
    1532                                                                                                                                    const btVector3& a,
    1533                                                                                                                                    const btVector3& b,
    1534                                                                                                                                    const btVector3& c,
    1535                                                                                                                                    btScalar maxt)
     1500                                        btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
     1501{
     1502o               =       org;
     1503d               =       dir;
     1504mint    =       mxt;
     1505face    =       0;
     1506tests   =       0;
     1507}
     1508
     1509//
     1510void                            btSoftBody::RayCaster::Process(const btDbvtNode* leaf)
     1511{
     1512btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
     1513const btScalar          t=rayTriangle(  o,d,
     1514                                                                        f.m_n[0]->m_x,
     1515                                                                        f.m_n[1]->m_x,
     1516                                                                        f.m_n[2]->m_x,
     1517                                                                        mint);
     1518if((t>0)&&(t<mint)) { mint=t;face=&f; }
     1519++tests;
     1520}
     1521
     1522//
     1523btScalar                        btSoftBody::RayCaster::rayTriangle(     const btVector3& org,
     1524                                                                                                                const btVector3& dir,
     1525                                                                                                                const btVector3& a,
     1526                                                                                                                const btVector3& b,
     1527                                                                                                                const btVector3& c,
     1528                                                                                                                btScalar maxt)
    15361529{
    15371530        static const btScalar   ceps=-SIMD_EPSILON*10;
    15381531        static const btScalar   teps=SIMD_EPSILON*10;
    1539 
    15401532        const btVector3                 n=cross(b-a,c-a);
    15411533        const btScalar                  d=dot(a,n);
    1542         const btScalar                  den=dot(rayNormalizedDirection,n);
     1534        const btScalar                  den=dot(dir,n);
    15431535        if(!btFuzzyZero(den))
    15441536        {
    1545                 const btScalar          num=dot(rayFrom,n)-d;
     1537                const btScalar          num=dot(org,n)-d;
    15461538                const btScalar          t=-num/den;
    15471539                if((t>teps)&&(t<maxt))
    15481540                {
    1549                         const btVector3 hit=rayFrom+rayNormalizedDirection*t;
     1541                        const btVector3 hit=org+dir*t;
    15501542                        if(     (dot(n,cross(a-hit,b-hit))>ceps)        &&                     
    15511543                                (dot(n,cross(b-hit,c-hit))>ceps)        &&
    … …  
    15691561        {
    15701562                if(m_nodes[i].m_leaf)
    1571                 {
     1563                        {
    15721564                        m_nodes[i].m_leaf->data=*(void**)&i;
    1573                 }
     1565                        }
    15741566        }
    15751567        for(i=0,ni=m_links.size();i<ni;++i)
    … …  
    15841576                m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
    15851577                if(m_faces[i].m_leaf)
    1586                 {
     1578                        {
    15871579                        m_faces[i].m_leaf->data=*(void**)&i;
    1588                 }
     1580                        }
    15891581        }
    15901582        for(i=0,ni=m_anchors.size();i<ni;++i)
    1591         {
     1583                {
    15921584                m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
    1593         }
     1585                }
    15941586        for(i=0,ni=m_notes.size();i<ni;++i)
    1595         {
     1587                {
    15961588                for(int j=0;j<m_notes[i].m_rank;++j)
    1597                 {
     1589                        {
    15981590                        m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
    1599                 }
    1600         }
     1591                        }
     1592                }
    16011593#undef  PTR2IDX
    16021594}
    … …  
    16061598{
    16071599#define IDX2PTR(_p_,_b_)        map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
    1608         (&(_b_)[(((char*)_p_)-(char*)0)])
     1600                                                                (&(_b_)[(((char*)_p_)-(char*)0)])
    16091601        btSoftBody::Node*       base=&m_nodes[0];
    16101602        int i,ni;
    … …  
    16131605        {
    16141606                if(m_nodes[i].m_leaf)
    1615                 {
     1607                        {
    16161608                        m_nodes[i].m_leaf->data=&m_nodes[i];
    1617                 }
     1609                        }
    16181610        }
    16191611        for(i=0,ni=m_links.size();i<ni;++i)
    … …  
    16281620                m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
    16291621                if(m_faces[i].m_leaf)
    1630                 {
     1622                        {
    16311623                        m_faces[i].m_leaf->data=&m_faces[i];
    1632                 }
     1624                        }
    16331625        }
    16341626        for(i=0,ni=m_anchors.size();i<ni;++i)
    1635         {
     1627                {
    16361628                m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
    1637         }
     1629                }
    16381630        for(i=0,ni=m_notes.size();i<ni;++i)
    1639         {
     1631                {
    16401632                for(int j=0;j<m_notes[i].m_rank;++j)
    1641                 {
     1633                        {
    16421634                        m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
    1643                 }
    1644         }
     1635                        }
     1636                }
    16451637#undef  IDX2PTR
    16461638}
    16471639
    16481640//
    1649 int                                     btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
     1641int                                     btSoftBody::rayCast(const btVector3& org,const btVector3& dir,
    16501642                                                                                btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
    16511643{
    16521644        int     cnt=0;
    16531645        if(bcountonly||m_fdbvt.empty())
    1654         {/* Full search */
    1655                 btVector3 dir = rayTo-rayFrom;
    1656                 dir.normalize();
    1657 
     1646                {/* Full search */
    16581647                for(int i=0,ni=m_faces.size();i<ni;++i)
    1659                 {
     1648                        {
    16601649                        const btSoftBody::Face& f=m_faces[i];
    1661 
    1662                         const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
    1663                                 f.m_n[0]->m_x,
    1664                                 f.m_n[1]->m_x,
    1665                                 f.m_n[2]->m_x,
    1666                                 mint);
     1650                        const btScalar                  t=RayCaster::rayTriangle(       org,dir,
     1651                                                                                                                                f.m_n[0]->m_x,
     1652                                                                                                                                f.m_n[1]->m_x,
     1653                                                                                                                                f.m_n[2]->m_x,
     1654                                                                                                                                mint);
    16671655                        if(t>0)
    1668                         {
     1656                                {
    16691657                                ++cnt;
    16701658                                if(!bcountonly)
    1671                                 {
     1659                                        {
    16721660                                        feature=btSoftBody::eFeature::Face;
    16731661                                        index=i;
    16741662                                        mint=t;
     1663                                        }
    16751664                                }
    16761665                        }
    16771666                }
    1678         }
    1679         else
    1680         {/* Use dbvt    */
    1681                 RayFromToCaster collider(rayFrom,rayTo,mint);
    1682 
    1683                 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
    1684                 if(collider.m_face)
    1685                 {
    1686                         mint=collider.m_mint;
     1667                else
     1668                {/* Use dbvt    */
     1669                RayCaster       collider(org,dir,mint);
     1670                btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider);
     1671                if(collider.face)
     1672                        {
     1673                        mint=collider.mint;
    16871674                        feature=btSoftBody::eFeature::Face;
    1688                         index=(int)(collider.m_face-&m_faces[0]);
     1675                        index=(int)(collider.face-&m_faces[0]);
    16891676                        cnt=1;
    1690                 }
    1691         }
     1677                        }
     1678                }
    16921679        return(cnt);
    16931680}
    … …  
    16961683void                    btSoftBody::initializeFaceTree()
    16971684{
    1698         m_fdbvt.clear();
    1699         for(int i=0;i<m_faces.size();++i)
    1700         {
    1701                 Face&   f=m_faces[i];
    1702                 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
     1685m_fdbvt.clear();
     1686for(int i=0;i<m_faces.size();++i)
     1687        {
     1688        Face&   f=m_faces[i];
     1689        f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
    17031690        }
    17041691}
    … …  
    17071694btVector3               btSoftBody::evaluateCom() const
    17081695{
    1709         btVector3       com(0,0,0);
    1710         if(m_pose.m_bframe)
    1711         {
    1712                 for(int i=0,ni=m_nodes.size();i<ni;++i)
    1713                 {
    1714                         com+=m_nodes[i].m_x*m_pose.m_wgh[i];
     1696btVector3       com(0,0,0);
     1697if(m_pose.m_bframe)
     1698        {
     1699        for(int i=0,ni=m_nodes.size();i<ni;++i)
     1700                {
     1701                com+=m_nodes[i].m_x*m_pose.m_wgh[i];
    17151702                }
    17161703        }
    17171704        return(com);
    17181705}
    1719 
     1706       
    17201707//
    17211708bool                            btSoftBody::checkContact(       btRigidBody* prb,
    1722                                                                                          const btVector3& x,
    1723                                                                                          btScalar margin,
    1724                                                                                          btSoftBody::sCti& cti) const
     1709                                                                                                const btVector3& x,
     1710                                                                                                btScalar margin,
     1711                                                                                                btSoftBody::sCti& cti) const
    17251712{
    17261713        btVector3                       nrm;
    … …  
    17281715        const btTransform&      wtr=prb->getInterpolationWorldTransform();
    17291716        btScalar                        dst=m_worldInfo->m_sparsesdf.Evaluate(  wtr.invXform(x),
    1730                 shp,
    1731                 nrm,
    1732                 margin);
     1717                                                                                                                                shp,
     1718                                                                                                                                nrm,
     1719                                                                                                                                margin);
    17331720        if(dst<0)
    17341721        {
    … …  
    17361723                cti.m_normal    =       wtr.getBasis()*nrm;
    17371724                cti.m_offset    =       -dot(   cti.m_normal,
    1738                         x-cti.m_normal*dst);
     1725                                                                        x-cti.m_normal*dst);
    17391726                return(true);
    17401727        }
    … …  
    17561743                btSoftBody::Face&       f=m_faces[i];
    17571744                const btVector3         n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
    1758                         f.m_n[2]->m_x-f.m_n[0]->m_x);
     1745                                                                        f.m_n[2]->m_x-f.m_n[0]->m_x);
    17591746                f.m_normal=n.normalized();
    17601747                f.m_n[0]->m_n+=n;
    … …  
    17741761{
    17751762        if(m_ndbvt.m_root)
    1776         {
     1763                {
    17771764                const btVector3&        mins=m_ndbvt.m_root->volume.Mins();
    17781765                const btVector3&        maxs=m_ndbvt.m_root->volume.Maxs();
    17791766                const btScalar          csm=getCollisionShape()->getMargin();
    17801767                const btVector3         mrg=btVector3(  csm,
    1781                         csm,
    1782                         csm)*1; // ??? to investigate...
     1768                                                                                        csm,
     1769                                                                                        csm)*1; // ??? to investigate...
    17831770                m_bounds[0]=mins-mrg;
    17841771                m_bounds[1]=maxs+mrg;
    1785                 if(0!=getBroadphaseHandle())
    1786                 {                                       
    1787                         m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
    1788                                 m_bounds[0],
    1789                                 m_bounds[1],
    1790                                 m_worldInfo->m_dispatcher);
    1791                 }
    1792         }
    1793         else
    1794         {
     1772                        if(0!=getBroadphaseHandle())
     1773                        {                                       
     1774                                m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
     1775                                                                                                        m_bounds[0],
     1776                                                                                                        m_bounds[1],
     1777                                                                                                        m_worldInfo->m_dispatcher);
     1778                        }
     1779                }
     1780                else
     1781                {
    17951782                m_bounds[0]=
    1796                         m_bounds[1]=btVector3(0,0,0);
    1797         }               
     1783                m_bounds[1]=btVector3(0,0,0);
     1784                }               
    17981785}
    17991786
    … …  
    18261813                pose.m_scl=pose.m_aqq*r.transpose()*Apq;
    18271814                if(m_cfg.maxvolume>1)
    1828                 {
     1815                        {
    18291816                        const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
    1830                                 1,m_cfg.maxvolume);
     1817                                                                                                        1,m_cfg.maxvolume);
    18311818                        pose.m_scl=Mul(pose.m_scl,idet);
    1832                 }
    1833 
     1819                        }
     1820               
    18341821        }
    18351822}
    … …  
    18861873        int i;
    18871874
    1888         for( i=0;i<m_clusters.size();++i)
    1889         {
    1890                 Cluster&        c=*m_clusters[i];
    1891                 c.m_imass=0;
    1892                 c.m_masses.resize(c.m_nodes.size());
    1893                 for(int j=0;j<c.m_nodes.size();++j)
    1894                 {
    1895                         c.m_masses[j]   =       c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
    1896                         c.m_imass               +=      c.m_masses[j];
    1897                 }
    1898                 c.m_imass               =       1/c.m_imass;
    1899                 c.m_com                 =       btSoftBody::clusterCom(&c);
    1900                 c.m_lv                  =       btVector3(0,0,0);
    1901                 c.m_av                  =       btVector3(0,0,0);
    1902                 c.m_leaf                =       0;
    1903                 /* Inertia      */
    1904                 btMatrix3x3&    ii=c.m_locii;
    1905                 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
    1906                 {
    1907                         int i,ni;
    1908 
    1909                         for(i=0,ni=c.m_nodes.size();i<ni;++i)
    1910                         {
    1911                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
    1912                                 const btVector3 q=k*k;
    1913                                 const btScalar  m=c.m_masses[i];
    1914                                 ii[0][0]        +=      m*(q[1]+q[2]);
    1915                                 ii[1][1]        +=      m*(q[0]+q[2]);
    1916                                 ii[2][2]        +=      m*(q[0]+q[1]);
    1917                                 ii[0][1]        -=      m*k[0]*k[1];
    1918                                 ii[0][2]        -=      m*k[0]*k[2];
    1919                                 ii[1][2]        -=      m*k[1]*k[2];
    1920                         }
    1921                 }
    1922                 ii[1][0]=ii[0][1];
    1923                 ii[2][0]=ii[0][2];
    1924                 ii[2][1]=ii[1][2];
    1925                 ii=ii.inverse();
    1926                 /* Frame        */
    1927                 c.m_framexform.setIdentity();
     1875for( i=0;i<m_clusters.size();++i)
     1876        {
     1877        Cluster&        c=*m_clusters[i];
     1878        c.m_imass=0;
     1879        c.m_masses.resize(c.m_nodes.size());
     1880        for(int j=0;j<c.m_nodes.size();++j)
     1881                {
     1882                c.m_masses[j]   =       c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
     1883                c.m_imass               +=      c.m_masses[j];
     1884                }
     1885        c.m_imass               =       1/c.m_imass;
     1886        c.m_com                 =       btSoftBody::clusterCom(&c);
     1887        c.m_lv                  =       btVector3(0,0,0);
     1888        c.m_av                  =       btVector3(0,0,0);
     1889        c.m_leaf                =       0;
     1890        /* Inertia      */
     1891        btMatrix3x3&    ii=c.m_locii;
     1892        ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
     1893        {
     1894                int i,ni;
     1895
     1896                for(i=0,ni=c.m_nodes.size();i<ni;++i)
     1897                {
     1898                        const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
     1899                        const btVector3 q=k*k;
     1900                        const btScalar  m=c.m_masses[i];
     1901                        ii[0][0]        +=      m*(q[1]+q[2]);
     1902                        ii[1][1]        +=      m*(q[0]+q[2]);
     1903                        ii[2][2]        +=      m*(q[0]+q[1]);
     1904                        ii[0][1]        -=      m*k[0]*k[1];
     1905                        ii[0][2]        -=      m*k[0]*k[2];
     1906                        ii[1][2]        -=      m*k[1]*k[2];
     1907                }
     1908        }
     1909        ii[1][0]=ii[0][1];
     1910        ii[2][0]=ii[0][2];
     1911        ii[2][1]=ii[1][2];
     1912        ii=ii.inverse();
     1913        /* Frame        */
     1914        c.m_framexform.setIdentity();
     1915        c.m_framexform.setOrigin(c.m_com);
     1916        c.m_framerefs.resize(c.m_nodes.size());
     1917        {
     1918                int i;
     1919                for(i=0;i<c.m_framerefs.size();++i)
     1920                        {
     1921                        c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
     1922                        }
     1923                }
     1924        }
     1925}
     1926
     1927//
     1928void                                    btSoftBody::updateClusters()
     1929{
     1930BT_PROFILE("UpdateClusters");
     1931int i;
     1932
     1933for(i=0;i<m_clusters.size();++i)
     1934        {
     1935        btSoftBody::Cluster&    c=*m_clusters[i];
     1936        const int                               n=c.m_nodes.size();
     1937        const btScalar                  invn=1/(btScalar)n;
     1938        if(n)
     1939                {
     1940                /* Frame                                */
     1941                const btScalar  eps=btScalar(0.0001);
     1942                btMatrix3x3             m,r,s;
     1943                m[0]=m[1]=m[2]=btVector3(0,0,0);
     1944                m[0][0]=eps*1;
     1945                m[1][1]=eps*2;
     1946                m[2][2]=eps*3;
     1947                c.m_com=clusterCom(&c);
     1948                for(int i=0;i<c.m_nodes.size();++i)
     1949                        {
     1950                        const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
     1951                        const btVector3&        b=c.m_framerefs[i];
     1952                        m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
     1953                        }
     1954                PolarDecompose(m,r,s);
    19281955                c.m_framexform.setOrigin(c.m_com);
    1929                 c.m_framerefs.resize(c.m_nodes.size());
    1930                 {
    1931                         int i;
    1932                         for(i=0;i<c.m_framerefs.size();++i)
    1933                         {
    1934                                 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
    1935                         }
    1936                 }
    1937         }
    1938 }
    1939 
    1940 //
    1941 void                                    btSoftBody::updateClusters()
    1942 {
    1943         BT_PROFILE("UpdateClusters");
    1944         int i;
    1945 
    1946         for(i=0;i<m_clusters.size();++i)
    1947         {
    1948                 btSoftBody::Cluster&    c=*m_clusters[i];
    1949                 const int                               n=c.m_nodes.size();
    1950                 const btScalar                  invn=1/(btScalar)n;
    1951                 if(n)
    1952                 {
    1953                         /* Frame                                */
    1954                         const btScalar  eps=btScalar(0.0001);
    1955                         btMatrix3x3             m,r,s;
    1956                         m[0]=m[1]=m[2]=btVector3(0,0,0);
    1957                         m[0][0]=eps*1;
    1958                         m[1][1]=eps*2;
    1959                         m[2][2]=eps*3;
    1960                         c.m_com=clusterCom(&c);
    1961                         for(int i=0;i<c.m_nodes.size();++i)
    1962                         {
    1963                                 const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
    1964                                 const btVector3&        b=c.m_framerefs[i];
    1965                                 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
    1966                         }
    1967                         PolarDecompose(m,r,s);
    1968                         c.m_framexform.setOrigin(c.m_com);
    1969                         c.m_framexform.setBasis(r);             
    1970                         /* Inertia                      */
    1971 #if 1/* Constant        */
    1972                         c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
    1973 #else
    1974 #if 0/* Sphere  */
     1956                c.m_framexform.setBasis(r);             
     1957                /* Inertia                      */
     1958                #if 1/* Constant        */
     1959                c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
     1960                #else
     1961                        #if 0/* Sphere  */
    19751962                        const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
    19761963                        const btVector3 inertia(rk,rk,rk);
    19771964                        const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
    1978                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
    1979                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
    1980 
     1965                                                                btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
     1966                                                                btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
     1967                       
    19811968                        c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
    1982 #else/* Actual  */             
     1969                        #else/* Actual  */             
    19831970                        c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
    19841971                        for(int i=0;i<n;++i)
    1985                         {
     1972                                {
    19861973                                const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
    19871974                                const btVector3         q=k*k;
    … …  
    19931980                                c.m_invwi[0][2] -=      m*k[0]*k[2];
    19941981                                c.m_invwi[1][2] -=      m*k[1]*k[2];
    1995                         }
     1982                                }
    19961983                        c.m_invwi[1][0]=c.m_invwi[0][1];
    19971984                        c.m_invwi[2][0]=c.m_invwi[0][2];
    19981985                        c.m_invwi[2][1]=c.m_invwi[1][2];
    19991986                        c.m_invwi=c.m_invwi.inverse();
    2000 #endif
    2001 #endif
    2002                         /* Velocities                   */
    2003                         c.m_lv=btVector3(0,0,0);
    2004                         c.m_av=btVector3(0,0,0);
    2005                         {
    2006                                 int i;
    2007 
    2008                                 for(i=0;i<n;++i)
     1987                        #endif
     1988                #endif
     1989                /* Velocities                   */
     1990                c.m_lv=btVector3(0,0,0);
     1991                c.m_av=btVector3(0,0,0);
     1992                {
     1993                        int i;
     1994
     1995                        for(i=0;i<n;++i)
     1996                        {
     1997                                const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
     1998                                c.m_lv  +=      v;
     1999                                c.m_av  +=      cross(c.m_nodes[i]->m_x-c.m_com,v);
     2000                        }
     2001                }
     2002                c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
     2003                c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
     2004                c.m_vimpulses[0]        =
     2005                c.m_vimpulses[1]        = btVector3(0,0,0);
     2006                c.m_dimpulses[0]        =
     2007                c.m_dimpulses[1]        = btVector3(0,0,0);
     2008                c.m_nvimpulses          = 0;
     2009                c.m_ndimpulses          = 0;
     2010                /* Matching                             */
     2011                if(c.m_matching>0)
     2012                        {
     2013                        for(int j=0;j<c.m_nodes.size();++j)
    20092014                                {
    2010                                         const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
    2011                                         c.m_lv  +=      v;
    2012                                         c.m_av  +=      cross(c.m_nodes[i]->m_x-c.m_com,v);
     2015                                Node&                   n=*c.m_nodes[j];
     2016                                const btVector3 x=c.m_framexform*c.m_framerefs[j];
     2017                                n.m_x=Lerp(n.m_x,x,c.m_matching);
    20132018                                }
    20142019                        }
    2015                         c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
    2016                         c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
    2017                         c.m_vimpulses[0]        =
    2018                                 c.m_vimpulses[1]        = btVector3(0,0,0);
    2019                         c.m_dimpulses[0]        =
    2020                                 c.m_dimpulses[1]        = btVector3(0,0,0);
    2021                         c.m_nvimpulses          = 0;
    2022                         c.m_ndimpulses          = 0;
    2023                         /* Matching                             */
    2024                         if(c.m_matching>0)
    2025                         {
    2026                                 for(int j=0;j<c.m_nodes.size();++j)
     2020                /* Dbvt                                 */
     2021                if(c.m_collide)
     2022                        {
     2023                        btVector3       mi=c.m_nodes[0]->m_x;
     2024                        btVector3       mx=mi;
     2025                        for(int j=1;j<n;++j)
    20272026                                {
    2028                                         Node&                   n=*c.m_nodes[j];
    2029                                         const btVector3 x=c.m_framexform*c.m_framerefs[j];
    2030                                         n.m_x=Lerp(n.m_x,x,c.m_matching);
     2027                                mi.setMin(c.m_nodes[j]->m_x);
     2028                                mx.setMax(c.m_nodes[j]->m_x);
     2029                                }                       
     2030                        const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
     2031                        if(c.m_leaf)
     2032                                m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
     2033                                else
     2034                                c.m_leaf=m_cdbvt.insert(bounds,&c);
     2035                        }
     2036                }
     2037        }
     2038}
     2039
     2040//
     2041void                                    btSoftBody::cleanupClusters()
     2042{
     2043for(int i=0;i<m_joints.size();++i)
     2044        {
     2045        m_joints[i]->Terminate(m_sst.sdt);
     2046        if(m_joints[i]->m_delete)
     2047                {
     2048                btAlignedFree(m_joints[i]);
     2049                m_joints.remove(m_joints[i--]);
     2050                }       
     2051        }
     2052}
     2053
     2054//
     2055void                                    btSoftBody::prepareClusters(int iterations)
     2056{
     2057for(int i=0;i<m_joints.size();++i)
     2058        {
     2059        m_joints[i]->Prepare(m_sst.sdt,iterations);
     2060        }
     2061}
     2062
     2063
     2064//
     2065void                                    btSoftBody::solveClusters(btScalar sor)
     2066{
     2067for(int i=0,ni=m_joints.size();i<ni;++i)
     2068        {
     2069        m_joints[i]->Solve(m_sst.sdt,sor);
     2070        }
     2071}
     2072
     2073//
     2074void                                    btSoftBody::applyClusters(bool drift)
     2075{
     2076BT_PROFILE("ApplyClusters");
     2077const btScalar                                  f0=m_sst.sdt;
     2078const btScalar                                  f1=f0/2;
     2079btAlignedObjectArray<btVector3> deltas;
     2080btAlignedObjectArray<btScalar>  weights;
     2081deltas.resize(m_nodes.size(),btVector3(0,0,0));
     2082weights.resize(m_nodes.size(),0);
     2083int i;
     2084
     2085if(drift)
     2086        {
     2087        for(i=0;i<m_clusters.size();++i)
     2088                {
     2089                Cluster&        c=*m_clusters[i];
     2090                if(c.m_ndimpulses)
     2091                        {
     2092                        c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
     2093                        c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
     2094                        }
     2095                }
     2096        }
     2097for(i=0;i<m_clusters.size();++i)
     2098        {
     2099        Cluster&        c=*m_clusters[i];       
     2100        if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
     2101                {
     2102                const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
     2103                const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
     2104                for(int j=0;j<c.m_nodes.size();++j)
     2105                        {
     2106                        const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
     2107                        const btVector3&        x=c.m_nodes[j]->m_x;
     2108                        const btScalar          q=c.m_masses[j];
     2109                        deltas[idx]             +=      (v+cross(w,x-c.m_com))*q;
     2110                        weights[idx]    +=      q;
     2111                        }
     2112                }
     2113        }
     2114        for(i=0;i<deltas.size();++i)
     2115        {
     2116                if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
     2117        }
     2118}
     2119
     2120//
     2121void                                    btSoftBody::dampClusters()
     2122{
     2123        int i;
     2124
     2125for(i=0;i<m_clusters.size();++i)
     2126        {
     2127        Cluster&        c=*m_clusters[i];       
     2128        if(c.m_ndamping>0)
     2129                {
     2130                for(int j=0;j<c.m_nodes.size();++j)
     2131                        {
     2132                        Node&                   n=*c.m_nodes[j];
     2133                        if(n.m_im>0)
     2134                                {
     2135                                const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
     2136                                n.m_v   +=      c.m_ndamping*(vx-n.m_v);
    20312137                                }
    20322138                        }
    2033                         /* Dbvt                                 */
    2034                         if(c.m_collide)
    2035                         {
    2036                                 btVector3       mi=c.m_nodes[0]->m_x;
    2037                                 btVector3       mx=mi;
    2038                                 for(int j=1;j<n;++j)
    2039                                 {
    2040                                         mi.setMin(c.m_nodes[j]->m_x);
    2041                                         mx.setMax(c.m_nodes[j]->m_x);
    2042                                 }                       
    2043                                 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
    2044                                 if(c.m_leaf)
    2045                                         m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
    2046                                 else
    2047                                         c.m_leaf=m_cdbvt.insert(bounds,&c);
    2048                         }
    2049                 }
    2050         }
    2051 }
    2052 
    2053 //
    2054 void                                    btSoftBody::cleanupClusters()
    2055 {
    2056         for(int i=0;i<m_joints.size();++i)
    2057         {
    2058                 m_joints[i]->Terminate(m_sst.sdt);
    2059                 if(m_joints[i]->m_delete)
    2060                 {
    2061                         btAlignedFree(m_joints[i]);
    2062                         m_joints.remove(m_joints[i--]);
    2063                 }       
    2064         }
    2065 }
    2066 
    2067 //
    2068 void                                    btSoftBody::prepareClusters(int iterations)
    2069 {
    2070         for(int i=0;i<m_joints.size();++i)
    2071         {
    2072                 m_joints[i]->Prepare(m_sst.sdt,iterations);
    2073         }
    2074 }
    2075 
    2076 
    2077 //
    2078 void                                    btSoftBody::solveClusters(btScalar sor)
    2079 {
    2080         for(int i=0,ni=m_joints.size();i<ni;++i)
    2081         {
    2082                 m_joints[i]->Solve(m_sst.sdt,sor);
    2083         }
    2084 }
    2085 
    2086 //
    2087 void                                    btSoftBody::applyClusters(bool drift)
    2088 {
    2089         BT_PROFILE("ApplyClusters");
    2090         const btScalar                                  f0=m_sst.sdt;
    2091         const btScalar                                  f1=f0/2;
    2092         btAlignedObjectArray<btVector3> deltas;
    2093         btAlignedObjectArray<btScalar>  weights;
    2094         deltas.resize(m_nodes.size(),btVector3(0,0,0));
    2095         weights.resize(m_nodes.size(),0);
    2096         int i;
    2097 
    2098         if(drift)
    2099         {
    2100                 for(i=0;i<m_clusters.size();++i)
    2101                 {
    2102                         Cluster&        c=*m_clusters[i];
    2103                         if(c.m_ndimpulses)
    2104                         {
    2105                                 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
    2106                                 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
    2107                         }
    2108                 }
    2109         }
    2110         for(i=0;i<m_clusters.size();++i)
    2111         {
    2112                 Cluster&        c=*m_clusters[i];       
    2113                 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
    2114                 {
    2115                         const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
    2116                         const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
    2117                         for(int j=0;j<c.m_nodes.size();++j)
    2118                         {
    2119                                 const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
    2120                                 const btVector3&        x=c.m_nodes[j]->m_x;
    2121                                 const btScalar          q=c.m_masses[j];
    2122                                 deltas[idx]             +=      (v+cross(w,x-c.m_com))*q;
    2123                                 weights[idx]    +=      q;
    2124                         }
    2125                 }
    2126         }
    2127         for(i=0;i<deltas.size();++i)
    2128         {
    2129                 if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
    2130         }
    2131 }
    2132 
    2133 //
    2134 void                                    btSoftBody::dampClusters()
    2135 {
    2136         int i;
    2137 
    2138         for(i=0;i<m_clusters.size();++i)
    2139         {
    2140                 Cluster&        c=*m_clusters[i];       
    2141                 if(c.m_ndamping>0)
    2142                 {
    2143                         for(int j=0;j<c.m_nodes.size();++j)
    2144                         {
    2145                                 Node&                   n=*c.m_nodes[j];
    2146                                 if(n.m_im>0)
    2147                                 {
    2148                                         const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
    2149                                         n.m_v   +=      c.m_ndamping*(vx-n.m_v);
    2150                                 }
    2151                         }
    21522139                }
    21532140        }
    … …  
    21572144void                            btSoftBody::Joint::Prepare(btScalar dt,int)
    21582145{
    2159         m_bodies[0].activate();
    2160         m_bodies[1].activate();
     2146m_bodies[0].activate();
     2147m_bodies[1].activate();
    21612148}
    21622149
    … …  
    21642151void                            btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
    21652152{
    2166         static const btScalar   maxdrift=4;
    2167         Joint::Prepare(dt,iterations);
    2168         m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
    2169         m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
    2170         m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
    2171         m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
    2172         m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
    2173         m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
    2174                 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
     2153static const btScalar   maxdrift=4;
     2154Joint::Prepare(dt,iterations);
     2155m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
     2156m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
     2157m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
     2158m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
     2159m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
     2160m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
     2161                                                                        m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
     2162if(m_split>0)
     2163        {
     2164        m_sdrift        =       m_massmatrix*(m_drift*m_split);
     2165        m_drift         *=      1-m_split;
     2166        }
     2167m_drift /=(btScalar)iterations;
     2168}
     2169
     2170//
     2171void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
     2172{
     2173const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
     2174const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
     2175const btVector3         vr=va-vb;
     2176btSoftBody::Impulse     impulse;
     2177impulse.m_asVelocity    =       1;
     2178impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
     2179m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
     2180m_bodies[1].applyImpulse( impulse,m_rpos[1]);
     2181}
     2182
     2183//
     2184void                            btSoftBody::LJoint::Terminate(btScalar dt)
     2185{
     2186if(m_split>0)
     2187        {
     2188        m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
     2189        m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
     2190        }
     2191}
     2192
     2193//
     2194void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
     2195{
     2196static const btScalar   maxdrift=SIMD_PI/16;
     2197m_icontrol->Prepare(this);
     2198Joint::Prepare(dt,iterations);
     2199m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
     2200m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
     2201m_drift         =       NormalizeAny(cross(m_axis[1],m_axis[0]));
     2202m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
     2203m_drift         *=      m_erp/dt;
     2204m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
     2205if(m_split>0)
     2206        {
     2207        m_sdrift        =       m_massmatrix*(m_drift*m_split);
     2208        m_drift         *=      1-m_split;
     2209        }
     2210m_drift /=(btScalar)iterations;
     2211}
     2212
     2213//
     2214void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
     2215{
     2216const btVector3         va=m_bodies[0].angularVelocity();
     2217const btVector3         vb=m_bodies[1].angularVelocity();
     2218const btVector3         vr=va-vb;
     2219const btScalar          sp=dot(vr,m_axis[0]);
     2220const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
     2221btSoftBody::Impulse     impulse;
     2222impulse.m_asVelocity    =       1;
     2223impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
     2224m_bodies[0].applyAImpulse(-impulse);
     2225m_bodies[1].applyAImpulse( impulse);
     2226}
     2227
     2228//
     2229void                            btSoftBody::AJoint::Terminate(btScalar dt)
     2230{
     2231if(m_split>0)
     2232        {
     2233        m_bodies[0].applyDAImpulse(-m_sdrift);
     2234        m_bodies[1].applyDAImpulse( m_sdrift);
     2235        }
     2236}
     2237
     2238//
     2239void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
     2240{
     2241Joint::Prepare(dt,iterations);
     2242const bool      dodrift=(m_life==0);
     2243m_delete=(++m_life)>m_maxlife;
     2244if(dodrift)
     2245        {
     2246        m_drift=m_drift*m_erp/dt;
    21752247        if(m_split>0)
    2176         {
     2248                {
    21772249                m_sdrift        =       m_massmatrix*(m_drift*m_split);
    21782250                m_drift         *=      1-m_split;
    2179         }
    2180         m_drift /=(btScalar)iterations;
    2181 }
    2182 
    2183 //
    2184 void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
    2185 {
    2186         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
    2187         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
    2188         const btVector3         vr=va-vb;
    2189         btSoftBody::Impulse     impulse;
    2190         impulse.m_asVelocity    =       1;
    2191         impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
    2192         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
    2193         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
    2194 }
    2195 
    2196 //
    2197 void                            btSoftBody::LJoint::Terminate(btScalar dt)
    2198 {
    2199         if(m_split>0)
    2200         {
    2201                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
    2202                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
    2203         }
    2204 }
    2205 
    2206 //
    2207 void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
    2208 {
    2209         static const btScalar   maxdrift=SIMD_PI/16;
    2210         m_icontrol->Prepare(this);
    2211         Joint::Prepare(dt,iterations);
    2212         m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
    2213         m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
    2214         m_drift         =       NormalizeAny(cross(m_axis[1],m_axis[0]));
    2215         m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
    2216         m_drift         *=      m_erp/dt;
    2217         m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
    2218         if(m_split>0)
    2219         {
    2220                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
    2221                 m_drift         *=      1-m_split;
    2222         }
    2223         m_drift /=(btScalar)iterations;
    2224 }
    2225 
    2226 //
    2227 void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
    2228 {
    2229         const btVector3         va=m_bodies[0].angularVelocity();
    2230         const btVector3         vb=m_bodies[1].angularVelocity();
    2231         const btVector3         vr=va-vb;
    2232         const btScalar          sp=dot(vr,m_axis[0]);
    2233         const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
    2234         btSoftBody::Impulse     impulse;
    2235         impulse.m_asVelocity    =       1;
    2236         impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
    2237         m_bodies[0].applyAImpulse(-impulse);
    2238         m_bodies[1].applyAImpulse( impulse);
    2239 }
    2240 
    2241 //
    2242 void                            btSoftBody::AJoint::Terminate(btScalar dt)
    2243 {
    2244         if(m_split>0)
    2245         {
    2246                 m_bodies[0].applyDAImpulse(-m_sdrift);
    2247                 m_bodies[1].applyDAImpulse( m_sdrift);
    2248         }
    2249 }
    2250 
    2251 //
    2252 void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
    2253 {
    2254         Joint::Prepare(dt,iterations);
    2255         const bool      dodrift=(m_life==0);
    2256         m_delete=(++m_life)>m_maxlife;
    2257         if(dodrift)
    2258         {
    2259                 m_drift=m_drift*m_erp/dt;
    2260                 if(m_split>0)
    2261                 {
    2262                         m_sdrift        =       m_massmatrix*(m_drift*m_split);
    2263                         m_drift         *=      1-m_split;
    2264                 }
    2265                 m_drift/=(btScalar)iterations;
     2251                }
     2252        m_drift/=(btScalar)iterations;
    22662253        }
    22672254        else
    22682255        {
    2269                 m_drift=m_sdrift=btVector3(0,0,0);
     2256        m_drift=m_sdrift=btVector3(0,0,0);
    22702257        }
    22712258}
    … …  
    22742261void                            btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
    22752262{
    2276         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
    2277         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
    2278         const btVector3         vrel=va-vb;
    2279         const btScalar          rvac=dot(vrel,m_normal);
    2280         btSoftBody::Impulse     impulse;
    2281         impulse.m_asVelocity    =       1;
    2282         impulse.m_velocity              =       m_drift;
    2283         if(rvac<0)
    2284         {
    2285                 const btVector3 iv=m_normal*rvac;
    2286                 const btVector3 fv=vrel-iv;
    2287                 impulse.m_velocity      +=      iv+fv*m_friction;
    2288         }
    2289         impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
    2290         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
    2291         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
     2263const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
     2264const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
     2265const btVector3         vrel=va-vb;
     2266const btScalar          rvac=dot(vrel,m_normal);
     2267btSoftBody::Impulse     impulse;
     2268impulse.m_asVelocity    =       1;
     2269impulse.m_velocity              =       m_drift;
     2270if(rvac<0)
     2271        {
     2272        const btVector3 iv=m_normal*rvac;
     2273        const btVector3 fv=vrel-iv;
     2274        impulse.m_velocity      +=      iv+fv*m_friction;
     2275        }
     2276impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
     2277m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
     2278m_bodies[1].applyImpulse( impulse,m_rpos[1]);
    22922279}
    22932280
    … …  
    22952282void                            btSoftBody::CJoint::Terminate(btScalar dt)
    22962283{
    2297         if(m_split>0)
    2298         {
    2299                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
    2300                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
     2284if(m_split>0)
     2285        {
     2286        m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
     2287        m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
    23012288        }
    23022289}
    … …  
    23162303        const bool                                              as_volume=kVC>0;
    23172304        const bool                                              as_aero=        as_lift         ||
    2318                 as_drag         ;
     2305                                                                                                as_drag         ;
    23192306        const bool                                              as_vaero=       as_aero         &&
    2320                 (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
     2307                                                                                                (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
    23212308        const bool                                              as_faero=       as_aero         &&
    2322                 (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
     2309                                                                                                (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
    23232310        const bool                                              use_medium=     as_aero;
    23242311        const bool                                              use_volume=     as_pressure     ||
    2325                 as_volume       ;
     2312                                                                                                as_volume       ;
    23262313        btScalar                                                volume=0;
    23272314        btScalar                                                ivolumetp=0;
    … …  
    24712458void                            btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
    24722459{
    2473         for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
    2474         {
    2475                 const SContact&         c=psb->m_scontacts[i];
    2476                 const btVector3&        nr=c.m_normal;
    2477                 Node&                           n=*c.m_node;
    2478                 Face&                           f=*c.m_face;
    2479                 const btVector3         p=BaryEval(     f.m_n[0]->m_x,
    2480                         f.m_n[1]->m_x,
    2481                         f.m_n[2]->m_x,
    2482                         c.m_weights);
    2483                 const btVector3         q=BaryEval(     f.m_n[0]->m_q,
    2484                         f.m_n[1]->m_q,
    2485                         f.m_n[2]->m_q,
    2486                         c.m_weights);                                                                                   
    2487                 const btVector3         vr=(n.m_x-n.m_q)-(p-q);
    2488                 btVector3                       corr(0,0,0);
    2489                 if(dot(vr,nr)<0)
    2490                 {
    2491                         const btScalar  j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
    2492                         corr+=c.m_normal*j;
    2493                 }
    2494                 corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
    2495                 n.m_x                   +=      corr*c.m_cfm[0];
    2496                 f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
    2497                 f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
    2498                 f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
     2460for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
     2461        {
     2462        const SContact&         c=psb->m_scontacts[i];
     2463        const btVector3&        nr=c.m_normal;
     2464        Node&                           n=*c.m_node;
     2465        Face&                           f=*c.m_face;
     2466        const btVector3         p=BaryEval(     f.m_n[0]->m_x,
     2467                                                                        f.m_n[1]->m_x,
     2468                                                                        f.m_n[2]->m_x,
     2469                                                                        c.m_weights);
     2470        const btVector3         q=BaryEval(     f.m_n[0]->m_q,
     2471                                                                        f.m_n[1]->m_q,
     2472                                                                        f.m_n[2]->m_q,
     2473                                                                        c.m_weights);                                                                                   
     2474        const btVector3         vr=(n.m_x-n.m_q)-(p-q);
     2475        btVector3                       corr(0,0,0);
     2476        if(dot(vr,nr)<0)
     2477                {
     2478                const btScalar  j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
     2479                corr+=c.m_normal*j;
     2480                }
     2481        corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
     2482        n.m_x                   +=      corr*c.m_cfm[0];
     2483        f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
     2484        f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
     2485        f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
    24992486        }
    25002487}
    … …  
    25232510void                            btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
    25242511{
    2525         for(int i=0,ni=psb->m_links.size();i<ni;++i)
     2512for(int i=0,ni=psb->m_links.size();i<ni;++i)
    25262513        {                       
    2527                 Link&                   l=psb->m_links[i];
    2528                 Node**                  n=l.m_n;
    2529                 const btScalar  j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
    2530                 n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
    2531                 n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
     2514        Link&                   l=psb->m_links[i];
     2515        Node**                  n=l.m_n;
     2516        const btScalar  j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
     2517        n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
     2518        n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
    25322519        }
    25332520}
    … …  
    25362523btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
    25372524{
    2538         switch(solver)
     2525switch(solver)
    25392526        {
    25402527        case    ePSolver::Anchors:              return(&btSoftBody::PSolve_Anchors);
    … …  
    25432530        case    ePSolver::SContacts:    return(&btSoftBody::PSolve_SContacts); 
    25442531        }
    2545         return(0);
     2532return(0);
    25462533}
    25472534
    … …  
    25492536btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
    25502537{
    2551         switch(solver)
     2538switch(solver)
    25522539        {
    25532540        case    eVSolver::Linear:               return(&btSoftBody::VSolve_Links);
    25542541        }
    2555         return(0);
     2542return(0);
    25562543}
    25572544
    … …  
    25592546void                    btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
    25602547{
    2561         switch(m_cfg.collisions&fCollision::RVSmask)
     2548switch(m_cfg.collisions&fCollision::RVSmask)
    25622549        {
    25632550        case    fCollision::SDF_RS:
    25642551                {
    2565                         btSoftColliders::CollideSDF_RS  docollide;             
    2566                         btRigidBody*            prb=btRigidBody::upcast(pco);
    2567                         const btTransform       wtr=prb->getInterpolationWorldTransform();
    2568                         const btTransform       ctr=prb->getWorldTransform();
    2569                         const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
    2570                         const btScalar          basemargin=getCollisionShape()->getMargin();
    2571                         btVector3                       mins;
    2572                         btVector3                       maxs;
    2573                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
    2574                         pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
    2575                                 mins,
    2576                                 maxs);
    2577                         volume=btDbvtVolume::FromMM(mins,maxs);
    2578                         volume.Expand(btVector3(basemargin,basemargin,basemargin));             
    2579                         docollide.psb           =       this;
    2580                         docollide.prb           =       prb;
    2581                         docollide.dynmargin     =       basemargin+timemargin;
    2582                         docollide.stamargin     =       basemargin;
    2583                         btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
    2584                 }
    2585                 break;
     2552                btSoftColliders::CollideSDF_RS  docollide;             
     2553                btRigidBody*            prb=btRigidBody::upcast(pco);
     2554                const btTransform       wtr=prb->getInterpolationWorldTransform();
     2555                const btTransform       ctr=prb->getWorldTransform();
     2556                const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
     2557                const btScalar          basemargin=getCollisionShape()->getMargin();
     2558                btVector3                       mins;
     2559                btVector3                       maxs;
     2560                ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
     2561                pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
     2562                                                                                        mins,
     2563                                                                                        maxs);
     2564                volume=btDbvtVolume::FromMM(mins,maxs);
     2565                volume.Expand(btVector3(basemargin,basemargin,basemargin));             
     2566                docollide.psb           =       this;
     2567                docollide.prb           =       prb;
     2568                docollide.dynmargin     =       basemargin+timemargin;
     2569                docollide.stamargin     =       basemargin;
     2570                btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
     2571                }
     2572        break;
    25862573        case    fCollision::CL_RS:
    25872574                {
    2588                         btSoftColliders::CollideCL_RS   collider;
    2589                         collider.Process(this,btRigidBody::upcast(pco));
    2590                 }
    2591                 break;
     2575                btSoftColliders::CollideCL_RS   collider;
     2576                collider.Process(this,btRigidBody::upcast(pco));
     2577                }
     2578        break;
    25922579        }
    25932580}
    … …  
    25962583void                    btSoftBody::defaultCollisionHandler(btSoftBody* psb)
    25972584{
    2598         const int cf=m_cfg.collisions&psb->m_cfg.collisions;
    2599         switch(cf&fCollision::SVSmask)
     2585const int cf=m_cfg.collisions&psb->m_cfg.collisions;
     2586switch(cf&fCollision::SVSmask)
    26002587        {
    26012588        case    fCollision::CL_SS:
    26022589                {
    2603                         btSoftColliders::CollideCL_SS   docollide;
    2604                         docollide.Process(this,psb);
    2605                 }
    2606                 break;
     2590                btSoftColliders::CollideCL_SS   docollide;
     2591                docollide.Process(this,psb);
     2592                }
     2593        break;
    26072594        case    fCollision::VF_SS:
    26082595                {
    2609                         btSoftColliders::CollideVF_SS   docollide;
    2610                         /* common                                       */
    2611                         docollide.mrg=  getCollisionShape()->getMargin()+
    2612                                 psb->getCollisionShape()->getMargin();
    2613                         /* psb0 nodes vs psb1 faces     */
    2614                         docollide.psb[0]=this;
    2615                         docollide.psb[1]=psb;
    2616                         btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
    2617                                 docollide.psb[1]->m_fdbvt.m_root,
    2618                                 docollide);
    2619                         /* psb1 nodes vs psb0 faces     */
    2620                         docollide.psb[0]=psb;
    2621                         docollide.psb[1]=this;
    2622                         btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
    2623                                 docollide.psb[1]->m_fdbvt.m_root,
    2624                                 docollide);
    2625                 }
    2626                 break;
    2627         }
    2628 }
     2596                btSoftColliders::CollideVF_SS   docollide;
     2597                /* common                                       */
     2598                docollide.mrg=  getCollisionShape()->getMargin()+
     2599                                                psb->getCollisionShape()->getMargin();
     2600                /* psb0 nodes vs psb1 faces     */
     2601                docollide.psb[0]=this;
     2602                docollide.psb[1]=psb;
     2603                btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
     2604                                                        docollide.psb[1]->m_fdbvt.m_root,
     2605                                                        docollide);
     2606                /* psb1 nodes vs psb0 faces     */
     2607                docollide.psb[0]=psb;
     2608                docollide.psb[1]=this;
     2609                btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
     2610                                                        docollide.psb[1]->m_fdbvt.m_root,
     2611                                                        docollide);
     2612                }
     2613        break;
     2614        }
     2615}

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.h

    r1963 r1972  
    6363                END
    6464        };};
    65 
     65       
    6666        ///eVSolver : velocities solvers
    6767        struct  eVSolver { enum _ {
    … …  
    6969                END
    7070        };};
    71 
     71       
    7272        ///ePSolver : positions solvers
    7373        struct  ePSolver { enum _ {
    … …  
    7878                END
    7979        };};
    80 
     80       
    8181        ///eSolverPresets
    8282        struct  eSolverPresets { enum _ {
    … …  
    8686                END
    8787        };};
    88 
     88       
    8989        ///eFeature
    9090        struct  eFeature { enum _ {
    … …  
    9595                END
    9696        };};
    97 
     97       
    9898        typedef btAlignedObjectArray<eVSolver::_>       tVSolverArray;
    9999        typedef btAlignedObjectArray<ePSolver::_>       tPSolverArray;
    100 
     100       
    101101        //
    102102        // Flags
    103103        //
    104 
     104       
    105105        ///fCollision
    106106        struct fCollision { enum _ {
    … …  
    108108                SDF_RS  =       0x0001, ///SDF based rigid vs soft
    109109                CL_RS   =       0x0002, ///Cluster vs convex rigid vs soft
    110 
     110               
    111111                SVSmask =       0x00f0, ///Rigid versus soft mask               
    112112                VF_SS   =       0x0010, ///Vertex vs face soft vs soft handling
    … …  
    116116                END
    117117        };};
    118 
     118       
    119119        ///fMaterial
    120120        struct fMaterial { enum _ {
    … …  
    124124                END
    125125        };};
    126 
     126               
    127127        //
    128128        // API Types
    129129        //
    130 
     130       
    131131        /* sRayCast             */
    132132        struct sRayCast
    … …  
    135135                eFeature::_     feature;        /// feature type
    136136                int                     index;          /// feature index
    137                 btScalar        fraction;               /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
    138         };
    139 
     137                btScalar        time;           /// time of impact (rayorg+raydir*time)
     138        };
     139       
    140140        /* ImplicitFn   */
    141141        struct  ImplicitFn
    … …  
    143143                virtual btScalar        Eval(const btVector3& x)=0;
    144144        };
    145 
     145       
    146146        //
    147147        // Internal types
    … …  
    181181                int                                             m_flags;                // Flags
    182182        };
    183 
     183               
    184184        /* Feature              */
    185185        struct  Feature : Element
    … …  
    297297                btScalar                                        m_matching;
    298298                bool                                            m_collide;
    299                 Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
     299                                                                        Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
    300300        };
    301301        /* Impulse              */
    … …  
    308308                Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0)       {}
    309309                Impulse                                         operator -() const
    310                 {
     310                        {
    311311                        Impulse i=*this;
    312312                        i.m_velocity=-i.m_velocity;
    313313                        i.m_drift=-i.m_drift;
    314314                        return(i);
    315                 }
     315                        }
    316316                Impulse                                         operator*(btScalar x) const
    317                 {
     317                        {
    318318                        Impulse i=*this;
    319319                        i.m_velocity*=x;
    320320                        i.m_drift*=x;
    321321                        return(i);
    322                 }
     322                        }
    323323        };
    324324        /* Body                 */
    … …  
    327327                Cluster*                m_soft;
    328328                btRigidBody*                            m_rigid;
    329                 Body() : m_soft(0),m_rigid(0)                           {}
    330                 Body(Cluster* p) : m_soft(p),m_rigid(0) {}
    331                 Body(btRigidBody* p) : m_soft(0),m_rigid(p)     {}
     329                                                                        Body() : m_soft(0),m_rigid(0)                           {}
     330                                                                        Body(Cluster* p) : m_soft(p),m_rigid(0) {}
     331                                                                        Body(btRigidBody* p) : m_soft(0),m_rigid(p)     {}
    332332                void                                            activate() const
    333                 {
     333                        {
    334334                        if(m_rigid) m_rigid->activate();
    335                 }
     335                        }
    336336                const btMatrix3x3&                      invWorldInertia() const
    337                 {
     337                        {
    338338                        static const btMatrix3x3        iwi(0,0,0,0,0,0,0,0,0);
    339339                        if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
    340340                        if(m_soft)      return(m_soft->m_invwi);
    341341                        return(iwi);
    342                 }
     342                        }
    343343                btScalar                                        invMass() const
    344                 {
     344                        {
    345345                        if(m_rigid) return(m_rigid->getInvMass());
    346346                        if(m_soft)      return(m_soft->m_imass);
    347347                        return(0);
    348                 }
     348                        }
    349349                const btTransform&                      xform() const
    350                 {
     350                        {
    351351                        static const btTransform        identity=btTransform::getIdentity();           
    352352                        if(m_rigid) return(m_rigid->getInterpolationWorldTransform());
    353353                        if(m_soft)      return(m_soft->m_framexform);
    354354                        return(identity);
    355                 }
     355                        }
    356356                btVector3                                       linearVelocity() const
    357                 {
     357                        {
    358358                        if(m_rigid) return(m_rigid->getLinearVelocity());
    359359                        if(m_soft)      return(m_soft->m_lv);
    360360                        return(btVector3(0,0,0));
    361                 }
     361                        }
    362362                btVector3                                       angularVelocity(const btVector3& rpos) const
    363                 {                       
     363                        {                       
    364364                        if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos));
    365365                        if(m_soft)      return(cross(m_soft->m_av,rpos));
    366366                        return(btVector3(0,0,0));
    367                 }
     367                        }
    368368                btVector3                                       angularVelocity() const
    369                 {                       
     369                        {                       
    370370                        if(m_rigid) return(m_rigid->getAngularVelocity());
    371371                        if(m_soft)      return(m_soft->m_av);
    372372                        return(btVector3(0,0,0));
    373                 }
     373                        }
    374374                btVector3                                       velocity(const btVector3& rpos) const
    375                 {
     375                        {
    376376                        return(linearVelocity()+angularVelocity(rpos));
    377                 }
     377                        }
    378378                void                                            applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
    379                 {
     379                        {
    380380                        if(m_rigid)     m_rigid->applyImpulse(impulse,rpos);
    381381                        if(m_soft)      btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
    382                 }
     382                        }
    383383                void                                            applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
    384                 {
     384                        {
    385385                        if(m_rigid)     m_rigid->applyImpulse(impulse,rpos);
    386386                        if(m_soft)      btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
    387                 }               
     387                        }               
    388388                void                                            applyImpulse(const Impulse& impulse,const btVector3& rpos) const
    389                 {
     389                        {
    390390                        if(impulse.m_asVelocity)        applyVImpulse(impulse.m_velocity,rpos);
    391391                        if(impulse.m_asDrift)           applyDImpulse(impulse.m_drift,rpos);
    392                 }
     392                        }
    393393                void                                            applyVAImpulse(const btVector3& impulse) const
    394                 {
     394                        {
    395395                        if(m_rigid)     m_rigid->applyTorqueImpulse(impulse);
    396396                        if(m_soft)      btSoftBody::clusterVAImpulse(m_soft,impulse);
    397                 }
     397                        }
    398398                void                                            applyDAImpulse(const btVector3& impulse) const
    399                 {
     399                        {
    400400                        if(m_rigid)     m_rigid->applyTorqueImpulse(impulse);
    401401                        if(m_soft)      btSoftBody::clusterDAImpulse(m_soft,impulse);
    402                 }
     402                        }
    403403                void                                            applyAImpulse(const Impulse& impulse) const
    404                 {
     404                        {
    405405                        if(impulse.m_asVelocity)        applyVAImpulse(impulse.m_velocity);
    406406                        if(impulse.m_asDrift)           applyDAImpulse(impulse.m_drift);
    407                 }
     407                        }
    408408                void                                            applyDCImpulse(const btVector3& impulse) const
    409                 {
     409                        {
    410410                        if(m_rigid)     m_rigid->applyCentralImpulse(impulse);
    411411                        if(m_soft)      btSoftBody::clusterDCImpulse(m_soft,impulse);
    412                 }
     412                        }
    413413        };
    414414        /* Joint                */
    … …  
    421421                };};
    422422                struct Specs
    423                 {
    424                         Specs() : erp(1),cfm(1),split(1) {}
     423                        {
     424                                                Specs() : erp(1),cfm(1),split(1) {}
    425425                        btScalar        erp;
    426426                        btScalar        cfm;
    427427                        btScalar        split;
    428                 };
     428                        };
    429429                Body                                            m_bodies[2];
    430430                btVector3                                       m_refs[2];
    … …  
    437437                bool                                            m_delete;
    438438                virtual                                         ~Joint() {}
    439                 Joint() : m_delete(false) {}
     439                                                                        Joint() : m_delete(false) {}
    440440                virtual void                            Prepare(btScalar dt,int iterations);
    441441                virtual void                            Solve(btScalar dt,btScalar sor)=0;
    … …  
    447447        {
    448448                struct Specs : Joint::Specs
    449                 {
     449                        {
    450450                        btVector3       position;
    451                 };             
     451                        };             
    452452                btVector3                                       m_rpos[2];
    453453                void                                            Prepare(btScalar dt,int iterations);
    … …  
    460460        {
    461461                struct IControl
    462                 {
     462                        {
    463463                        virtual void                    Prepare(AJoint*)                                {}
    464464                        virtual btScalar                Speed(AJoint*,btScalar current) { return(current); }
    465465                        static IControl*                Default()                                               { static IControl def;return(&def); }
    466                 };
     466                        };
    467467                struct Specs : Joint::Specs
    468                 {
    469                         Specs() : icontrol(IControl::Default()) {}
     468                        {
     469                                                Specs() : icontrol(IControl::Default()) {}
    470470                        btVector3       axis;
    471471                        IControl*       icontrol;
    472                 };             
     472                        };             
    473473                btVector3                                       m_axis[2];
    474474                IControl*                                       m_icontrol;
    … …  
    533533                btScalar                                updmrg;                 // Update margin
    534534        };     
    535         /// RayFromToCaster takes a ray from, ray to (instead of direction!)
    536         struct  RayFromToCaster : btDbvt::ICollide
    537         {
    538                 btVector3                       m_rayFrom;
    539                 btVector3                       m_rayTo;
    540                 btVector3                       m_rayNormalizedDirection;
    541                 btScalar                        m_mint;
    542                 Face*                           m_face;
    543                 int                                     m_tests;
    544                 RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
     535        /* RayCaster    */
     536        struct  RayCaster : btDbvt::ICollide
     537                {
     538                btVector3                       o;
     539                btVector3                       d;
     540                btScalar                        mint;
     541                Face*   face;
     542                int                                     tests;
     543                                                                RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt);
    545544                void                                    Process(const btDbvtNode* leaf);
    546 
    547                 static inline btScalar  rayFromToTriangle(const btVector3& rayFrom,
    548                         const btVector3& rayTo,
    549                         const btVector3& rayNormalizedDirection,
    550                         const btVector3& a,
    551                         const btVector3& b,
    552                         const btVector3& c,
    553                         btScalar maxt=SIMD_INFINITY);
    554         };
     545                static inline btScalar  rayTriangle(const btVector3& org,
     546                                                                                        const btVector3& dir,
     547                                                                                        const btVector3& a,
     548                                                                                        const btVector3& b,
     549                                                                                        const btVector3& c,
     550                                                                                        btScalar maxt=SIMD_INFINITY);
     551                };
    555552
    556553        //
    … …  
    598595        btDbvt                                  m_cdbvt;                // Clusters tree
    599596        tClusterArray                   m_clusters;             // Clusters
    600 
     597               
    601598        //
    602599        // Api
    603600        //
    604 
     601       
    605602        /* ctor                                                                                                                                 */
    606603        btSoftBody(     btSoftBodyWorldInfo* worldInfo,int node_count,
    607                 const btVector3* x,
    608                 const btScalar* m);
     604                                const btVector3* x,
     605                                const btScalar* m);
    609606        /* dtor                                                                                                                                 */
    610607        virtual ~btSoftBody();
    … …  
    626623                int node1) const;
    627624        bool                            checkLink(      const Node* node0,
    628                 const Node* node1) const;
     625                                                                        const Node* node1) const;
    629626        /* Check for existring face                                                                                             */
    630627        bool                            checkFace(      int node0,
    631                 int node1,
    632                 int node2) const;
     628                                                                        int node1,
     629                                                                        int node2) const;
    633630        /* Append material                                                                                                              */
    634631        Material*                       appendMaterial();
    635632        /* Append note                                                                                                                  */
    636633        void                            appendNote(     const char* text,
    637                 const btVector3& o,
    638                 const btVector4& c=btVector4(1,0,0,0),
    639                 Node* n0=0,
    640                 Node* n1=0,
    641                 Node* n2=0,
    642                 Node* n3=0);
     634                                                                        const btVector3& o,
     635                                                                        const btVector4& c=btVector4(1,0,0,0),
     636                                                                        Node* n0=0,
     637                                                                        Node* n1=0,
     638                                                                        Node* n2=0,
     639                                                                        Node* n3=0);
    643640        void                            appendNote(     const char* text,
    644                 const btVector3& o,
    645                 Node* feature);
     641                                                                        const btVector3& o,
     642                                                                        Node* feature);
    646643        void                            appendNote(     const char* text,
    647                 const btVector3& o,
    648                 Link* feature);
     644                                                                        const btVector3& o,
     645                                                                        Link* feature);
    649646        void                            appendNote(     const char* text,
    650                 const btVector3& o,
    651                 Face* feature);
     647                                                                        const btVector3& o,
     648                                                                        Face* feature);
    652649        /* Append node                                                                                                                  */
    653650        void                            appendNode(     const btVector3& x,btScalar m);
    … …  
    655652        void                            appendLink(int model=-1,Material* mat=0);
    656653        void                            appendLink(     int node0,
    657                 int node1,
    658                 Material* mat=0,
    659                 bool bcheckexist=false);
     654                                                                        int node1,
     655                                                                        Material* mat=0,
     656                                                                        bool bcheckexist=false);
    660657        void                            appendLink(     Node* node0,
    661                 Node* node1,
    662                 Material* mat=0,
    663                 bool bcheckexist=false);
     658                                                                        Node* node1,
     659                                                                        Material* mat=0,
     660                                                                        bool bcheckexist=false);
    664661        /* Append face                                                                                                                  */
    665662        void                            appendFace(int model=-1,Material* mat=0);
    666663        void                            appendFace(     int node0,
    667                 int node1,
    668                 int node2,
    669                 Material* mat=0);
     664                                                                        int node1,
     665                                                                        int node2,
     666                                                                        Material* mat=0);
    670667        /* Append anchor                                                                                                                */
    671668        void                            appendAnchor(   int node,
    672                 btRigidBody* body);
     669                                                                                btRigidBody* body);
    673670        /* Append linear joint                                                                                                  */
    674671        void                            appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
    … …  
    683680        /* Add force (or gravity) to a node of the body                                                 */
    684681        void                            addForce(               const btVector3& force,
    685                 int node);
     682                                                                                int node);
    686683        /* Add velocity to the entire body                                                                              */
    687684        void                            addVelocity(    const btVector3& velocity);
    … …  
    692689        /* Add velocity to a node of the body                                                                   */
    693690        void                            addVelocity(    const btVector3& velocity,
    694                 int node);
     691                                                                                int node);
    695692        /* Set mass                                                                                                                             */
    696693        void                            setMass(                int node,
    697                 btScalar mass);
     694                                                                                btScalar mass);
    698695        /* Get mass                                                                                                                             */
    699696        btScalar                        getMass(                int node) const;
    … …  
    702699        /* Set total mass (weighted by previous masses)                                                 */
    703700        void                            setTotalMass(   btScalar mass,
    704                 bool fromfaces=false);
     701                                                                                bool fromfaces=false);
    705702        /* Set total density                                                                                                    */
    706703        void                            setTotalDensity(btScalar density);
    … …  
    715712        /* Set current state as pose                                                                                    */
    716713        void                            setPose(                bool bvolume,
    717                 bool bframe);
     714                                                                                bool bframe);
    718715        /* Return the volume                                                                                                    */
    719716        btScalar                        getVolume() const;
    … …  
    735732        /* Generate bending constraints based on distance in the adjency graph  */
    736733        int                                     generateBendingConstraints(     int distance,
    737                 Material* mat=0);
     734                                                                                                        Material* mat=0);
    738735        /* Randomize constraints to reduce solver bias                                                  */
    739736        void                            randomizeConstraints();
    … …  
    748745        bool                            cutLink(int node0,int node1,btScalar position);
    749746        bool                            cutLink(const Node* node0,const Node* node1,btScalar position);
    750 
    751         ///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
    752         bool                            rayTest(const btVector3& rayFrom,
    753                 const btVector3& rayTo,
    754                 sRayCast& results);
     747        /* Ray casting                                                                                                                  */
     748        bool                            rayCast(const btVector3& org,
     749                                                                const btVector3& dir,
     750                                                                sRayCast& results,
     751                                                                btScalar maxtime=SIMD_INFINITY);
    755752        /* Solver presets                                                                                                               */
    756753        void                            setSolver(eSolverPresets::_ preset);
    … …  
    770767        void                            defaultCollisionHandler(btCollisionObject* pco);
    771768        void                            defaultCollisionHandler(btSoftBody* psb);
    772 
     769               
    773770        //
    774771        // Cast
    775772        //
    776 
     773               
    777774        static const btSoftBody*        upcast(const btCollisionObject* colObj)
    778775        {
    … …  
    802799        void                            pointersToIndices();
    803800        void                            indicesToPointers(const int* map=0);
    804 
    805         int                                     rayTest(const btVector3& rayFrom,const btVector3& rayTo,
    806                 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
     801        int                                     rayCast(const btVector3& org,const btVector3& dir,
     802                                                                btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
    807803        void                            initializeFaceTree();
    808804        btVector3                       evaluateCom() const;
    … …  
    827823        static psolver_t        getSolver(ePSolver::_ solver);
    828824        static vsolver_t        getSolver(eVSolver::_ solver);
    829 
     825       
    830826};
    831827

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp

    r1963 r1972  
    5151
    5252btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
    53 m_dispatcher(dispatcher),
    54 m_dispatchInfoPtr(0)
     53          m_dispatcher(dispatcher),
     54        m_dispatchInfoPtr(0)
    5555{
    5656        m_softBody = (btSoftBody*) (isSwapped? body1:body0);
    5757        m_triBody = isSwapped? body0:body1;
    58 
    59         //
    60         // create the manifold from the dispatcher 'manifold pool'
    61         //
    62         //        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
    63 
     58       
     59          //
     60          // create the manifold from the dispatcher 'manifold pool'
     61          //
     62//        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
     63
     64          clearCache();
     65}
     66
     67btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
     68{
    6469        clearCache();
    65 }
    66 
    67 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
    68 {
    69         clearCache();
    70         //      m_dispatcher->releaseManifold( m_manifoldPtr );
    71 
    72 }
    73 
     70//      m_dispatcher->releaseManifold( m_manifoldPtr );
     71 
     72}
     73 
    7474
    7575void    btSoftBodyTriangleCallback::clearCache()
    … …  
    8989void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
    9090{
    91         //just for debugging purposes
     91        //just for debugging purposes
    9292        //printf("triangle %d",m_triangleCount++);
    9393        btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
    … …  
    9595        ci.m_dispatcher1 = m_dispatcher;
    9696
    97         ///debug drawing of the overlapping triangles
     97                ///debug drawing of the overlapping triangles
    9898        if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
    9999        {
    … …  
    108108        btHashKey<btTriIndex> triKey(triIndex.getUid());
    109109
    110 
     110       
    111111        btTriIndex* shapeIndex = m_shapeCache[triKey];
    112112        if (shapeIndex)
    … …  
    117117                //copy over user pointers to temporary shape
    118118                tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
    119 
     119               
    120120                btCollisionShape* tmpShape = ob->getCollisionShape();
    121121                ob->internalSetTemporaryCollisionShape( tm );
    122 
     122               
    123123
    124124                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
    125 
     125               
    126126                colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
    127127                colAlgo->~btCollisionAlgorithm();
    … …  
    134134
    135135        //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
    136 
    137         //      if (m_softBody->getCollisionShape()->getShapeType()==
    138         {
    139                 //              btVector3 other;
     136       
     137//      if (m_softBody->getCollisionShape()->getShapeType()==
     138        {
     139//              btVector3 other;
    140140                btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
    141141                normal.normalize();
    142142                normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
    143                 //              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
    144                 //              other+=normal*22.f;
     143//              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
     144//              other+=normal*22.f;
    145145                btVector3       pts[6] = {triangle[0]+normal,
    146146                        triangle[1]+normal,
    147                         triangle[2]+normal,
    148                         triangle[0]-normal,
    149                         triangle[1]-normal,
    150                         triangle[2]-normal};
     147                triangle[2]+normal,
     148                triangle[0]-normal,
     149                triangle[1]-normal,
     150                triangle[2]-normal};
    151151
    152152                btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
    153153
    154154
    155                 //              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
    156 
     155//              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
     156               
    157157                //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);     
    158                 //      tm.setMargin(m_collisionMarginTriangle);
    159 
     158        //      tm.setMargin(m_collisionMarginTriangle);
     159               
    160160                //copy over user pointers to temporary shape
    161161                tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
    162 
     162               
    163163                btCollisionShape* tmpShape = ob->getCollisionShape();
    164164                ob->internalSetTemporaryCollisionShape( tm );
    165 
     165               
    166166
    167167                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
    … …  
    170170
    171171                //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
    172                 //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
    173                 //              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
     172        //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
     173//              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
    174174                colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
    175175                colAlgo->~btCollisionAlgorithm();
    176176                ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
    177 
    178 
     177               
     178               
    179179                ob->internalSetTemporaryCollisionShape( tmpShape );
    180180                triIndex.m_childShape = tm;
    … …  
    183183        }
    184184
    185 
     185       
    186186
    187187}
    … …  
    195195        m_resultOut = resultOut;
    196196
    197 
     197       
    198198        btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
    199199        m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
    … …  
    218218void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
    219219{
    220 
    221 
     220       
     221       
    222222        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
    223223        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
    … …  
    229229                btCollisionObject*      triOb = triBody;
    230230                btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
    231 
    232                 //      if (convexBody->getCollisionShape()->isConvex())
     231               
     232        //      if (convexBody->getCollisionShape()->isConvex())
    233233                {
    234234                        btScalar collisionMarginTriangle = concaveShape->getMargin();
    235 
    236                         //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
     235                                       
     236//                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
    237237                        m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
    238238
    … …  
    240240                        //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
    241241
    242                         //                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
     242//                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
    243243
    244244
    245245                        concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
    246 
    247                         //      resultOut->refreshContactPoints();
    248 
    249                 }
    250 
     246                       
     247                //      resultOut->refreshContactPoints();
     248       
     249                }
     250       
    251251        }
    252252
    … …  
    288288                btScalar        m_ccdSphereRadius;
    289289                btScalar        m_hitFraction;
    290 
     290       
    291291
    292292                LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
    … …  
    297297                {                       
    298298                }
    299 
    300 
     299               
     300               
    301301                virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
    302302                {
    … …  
    328328
    329329
    330 
    331 
    332 
     330       
     331
     332       
    333333        if (triBody->getCollisionShape()->isConcave())
    334334        {
    … …  
    350350
    351351                btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
    352 
     352               
    353353                if (triangleMesh)
    354354                {
    355355                        triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
    356356                }
    357 
     357       
    358358
    359359

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h

    r1963 r1972  
    3636        int m_PartIdTriangleIndex;
    3737        class btCollisionShape* m_childShape;
    38 
     38       
    3939        btTriIndex(int partId,int triangleIndex,btCollisionShape* shape)
    4040        {
    … …  
    7676
    7777        btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
    78 
     78       
    7979public:
    80         int     m_triangleCount;
    81 
    82         //      btPersistentManifold*   m_manifoldPtr;
     80int     m_triangleCount;
     81       
     82//      btPersistentManifold*   m_manifoldPtr;
    8383
    8484        btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
    … …  
    8989
    9090        virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
    91 
     91       
    9292        void clearCache();
    9393

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

    r1963 r1972  
    2323//
    2424static void                             drawVertex(     btIDebugDraw* idraw,
    25                                                                    const btVector3& x,btScalar s,const btVector3& c)
    26 {
    27         idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
    28         idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
    29         idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
    30 }
     25                                                                        const btVector3& x,btScalar s,const btVector3& c)
     26        {
     27                idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
     28                idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
     29                idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
     30        }
    3131
    3232//
    3333static void                             drawBox(        btIDebugDraw* idraw,
    34                                                                 const btVector3& mins,
    35                                                                 const btVector3& maxs,
    36                                                                 const btVector3& color)
    37 {
    38         const btVector3 c[]={   btVector3(mins.x(),mins.y(),mins.z()),
    39                 btVector3(maxs.x(),mins.y(),mins.z()),
    40                 btVector3(maxs.x(),maxs.y(),mins.z()),
    41                 btVector3(mins.x(),maxs.y(),mins.z()),
    42                 btVector3(mins.x(),mins.y(),maxs.z()),
    43                 btVector3(maxs.x(),mins.y(),maxs.z()),
    44                 btVector3(maxs.x(),maxs.y(),maxs.z()),
    45                 btVector3(mins.x(),maxs.y(),maxs.z())};
    46         idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
    47         idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
    48         idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
    49         idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
    50         idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
    51         idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
     34                                                                        const btVector3& mins,
     35                                                                        const btVector3& maxs,
     36                                                                        const btVector3& color)
     37{
     38const btVector3 c[]={   btVector3(mins.x(),mins.y(),mins.z()),
     39                                                btVector3(maxs.x(),mins.y(),mins.z()),
     40                                                btVector3(maxs.x(),maxs.y(),mins.z()),
     41                                                btVector3(mins.x(),maxs.y(),mins.z()),
     42                                                btVector3(mins.x(),mins.y(),maxs.z()),
     43                                                btVector3(maxs.x(),mins.y(),maxs.z()),
     44                                                btVector3(maxs.x(),maxs.y(),maxs.z()),
     45                                                btVector3(mins.x(),maxs.y(),maxs.z())};
     46idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
     47idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
     48idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
     49idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
     50idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
     51idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
    5252}
    5353
    5454//
    5555static void                             drawTree(       btIDebugDraw* idraw,
    56                                                                  const btDbvtNode* node,
    57                                                                  int depth,
    58                                                                  const btVector3& ncolor,
    59                                                                  const btVector3& lcolor,
    60                                                                  int mindepth,
    61                                                                  int maxdepth)
    62 {
    63         if(node)
    64         {
    65                 if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
    66                 {
    67                         drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
    68                         drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
    69                 }
    70                 if(depth>=mindepth)
    71                 {
    72                         const btScalar  scl=(btScalar)(node->isinternal()?1:1);
    73                         const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
    74                         const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
    75                         drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
     56                                                                        const btDbvtNode* node,
     57                                                                        int depth,
     58                                                                        const btVector3& ncolor,
     59                                                                        const btVector3& lcolor,
     60                                                                        int mindepth,
     61                                                                        int maxdepth)
     62{
     63if(node)
     64        {
     65        if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
     66                {
     67                drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
     68                drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
     69                }
     70        if(depth>=mindepth)
     71                {
     72                const btScalar  scl=(btScalar)(node->isinternal()?1:1);
     73                const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
     74                const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
     75                drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
    7676                }
    7777        }
    … …  
    8282static inline T                         sum(const btAlignedObjectArray<T>& items)
    8383{
    84         T       v;
    85         if(items.size())
    86         {
    87                 v=items[0];
    88                 for(int i=1,ni=items.size();i<ni;++i)
    89                 {
    90                         v+=items[i];
    91                 }
    92         }
    93         return(v);
     84T       v;
     85if(items.size())
     86        {
     87        v=items[0];
     88        for(int i=1,ni=items.size();i<ni;++i)
     89                {
     90                v+=items[i];
     91                }
     92        }
     93return(v);
    9494}
    9595
    … …  
    9898static inline void                      add(btAlignedObjectArray<T>& items,const Q& value)
    9999{
    100         for(int i=0,ni=items.size();i<ni;++i)
    101         {
    102                 items[i]+=value;
     100for(int i=0,ni=items.size();i<ni;++i)
     101        {
     102        items[i]+=value;
    103103        }
    104104}
    … …  
    108108static inline void                      mul(btAlignedObjectArray<T>& items,const Q& value)
    109109{
    110         for(int i=0,ni=items.size();i<ni;++i)
    111         {
    112                 items[i]*=value;
     110for(int i=0,ni=items.size();i<ni;++i)
     111        {
     112        items[i]*=value;
    113113        }
    114114}
    … …  
    118118static inline T                         average(const btAlignedObjectArray<T>& items)
    119119{
    120         const btScalar  n=(btScalar)(items.size()>0?items.size():1);
    121         return(sum(items)/n);
     120const btScalar  n=(btScalar)(items.size()>0?items.size():1);
     121return(sum(items)/n);
    122122}
    123123
    … …  
    137137#if 0
    138138static btVector3                stresscolor(btScalar stress)
    139 {
     139        {
    140140        static const btVector3  spectrum[]=     {       btVector3(1,0,1),
    141                 btVector3(0,0,1),
    142                 btVector3(0,1,1),
    143                 btVector3(0,1,0),
    144                 btVector3(1,1,0),
    145                 btVector3(1,0,0),
    146                 btVector3(1,0,0)};
     141                                                                                        btVector3(0,0,1),
     142                                                                                        btVector3(0,1,1),
     143                                                                                        btVector3(0,1,0),
     144                                                                                        btVector3(1,1,0),
     145                                                                                        btVector3(1,0,0),
     146                                                                                        btVector3(1,0,0)};
    147147        static const int                ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
    148148        static const btScalar   one=1;
    … …  
    151151        const btScalar                  frc=stress-sel;
    152152        return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
    153 }
     153        }
    154154#endif
    155155
    156156//
    157157void                    btSoftBodyHelpers::Draw(        btSoftBody* psb,
    158                                                                                 btIDebugDraw* idraw,
    159                                                                                 int drawflags)
     158                                         btIDebugDraw* idraw,
     159                                         int drawflags)
    160160{
    161161        const btScalar          scl=(btScalar)0.1;
    … …  
    252252                        const btVector3                 c=(x[0]+x[1]+x[2])/3;
    253253                        idraw->drawTriangle((x[0]-c)*scl+c,
    254                                 (x[1]-c)*scl+c,
    255                                 (x[2]-c)*scl+c,
    256                                 col,alp);
     254                                                                (x[1]-c)*scl+c,
     255                                                                (x[2]-c)*scl+c,
     256                                                                col,alp);
    257257                }       
    258258        }
    259259        /* Clusters     */
    260260        if(0!=(drawflags&fDrawFlags::Clusters))
    261         {
     261                {
    262262                srand(1806);
    263263                for(i=0;i<psb->m_clusters.size();++i)
    264                 {
     264                        {
    265265                        if(psb->m_clusters[i]->m_collide)
    266                         {
     266                                {
    267267                                btVector3                                               color(  rand()/(btScalar)RAND_MAX,
    268                                         rand()/(btScalar)RAND_MAX,
    269                                         rand()/(btScalar)RAND_MAX);
     268                                                                                                                rand()/(btScalar)RAND_MAX,
     269                                                                                                                rand()/(btScalar)RAND_MAX);
    270270                                color=color.normalized()*0.75;
    271271                                btAlignedObjectArray<btVector3> vertices;
    272272                                vertices.resize(psb->m_clusters[i]->m_nodes.size());
    273273                                for(j=0,nj=vertices.size();j<nj;++j)
    274                                 {                               
     274                                        {                               
    275275                                        vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
    276                                 }
     276                                        }
    277277                                HullDesc                hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
    278278                                HullResult              hres;
    … …  
    285285                                add(hres.m_OutputVertices,center);
    286286                                for(j=0;j<(int)hres.mNumFaces;++j)
    287                                 {
     287                                        {
    288288                                        const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
    289289                                        idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
    290                                                 hres.m_OutputVertices[idx[1]],
    291                                                 hres.m_OutputVertices[idx[2]],
    292                                                 color,1);
     290                                                                                hres.m_OutputVertices[idx[1]],
     291                                                                                hres.m_OutputVertices[idx[2]],
     292                                                                                color,1);
     293                                        }
     294                                hlib.ReleaseResult(hres);
    293295                                }
    294                                 hlib.ReleaseResult(hres);
    295                         }
    296296                        /* Velocities   */
    297 #if 0
     297                        #if 0
    298298                        for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
    299                         {
     299                                {
    300300                                const btSoftBody::Cluster&      c=psb->m_clusters[i];
    301301                                const btVector3                         r=c.m_nodes[j]->m_x-c.m_com;
    302302                                const btVector3                         v=c.m_lv+cross(c.m_av,r);
    303303                                idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
    304                         }
    305 #endif
     304                                }
     305                        #endif
    306306                        /* Frame                */
    307307                        btSoftBody::Cluster& c=*psb->m_clusters[i];
    … …  
    309309                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
    310310                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
    311                 }
    312         }
     311                        }
     312                }
    313313        /* Notes        */
    314314        if(0!=(drawflags&fDrawFlags::Notes))
    … …  
    319319                        btVector3                               p=n.m_offset;
    320320                        for(int j=0;j<n.m_rank;++j)
    321                         {
     321                                {
    322322                                p+=n.m_nodes[j]->m_x*n.m_coords[j];
    323                         }
     323                                }
    324324                        idraw->draw3dText(p,n.m_text);
    325325                }
    … …  
    334334        if(0!=(drawflags&fDrawFlags::Joints))
    335335        {
    336                 for(i=0;i<psb->m_joints.size();++i)
    337                 {
    338                         const btSoftBody::Joint*        pj=psb->m_joints[i];
    339                         switch(pj->Type())
     336        for(i=0;i<psb->m_joints.size();++i)
     337                {
     338                const btSoftBody::Joint*        pj=psb->m_joints[i];
     339                switch(pj->Type())
    340340                        {
    341341                        case    btSoftBody::Joint::eType::Linear:
    342342                                {
    343                                         const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
    344                                         const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
    345                                         const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
    346                                         idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
    347                                         idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
    348                                         drawVertex(idraw,a0,0.25,btVector3(1,1,0));
    349                                         drawVertex(idraw,a1,0.25,btVector3(0,1,1));
     343                                const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
     344                                const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
     345                                const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
     346                                idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
     347                                idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
     348                                drawVertex(idraw,a0,0.25,btVector3(1,1,0));
     349                                drawVertex(idraw,a1,0.25,btVector3(0,1,1));
    350350                                }
    351                                 break;
     351                        break;
    352352                        case    btSoftBody::Joint::eType::Angular:
    353353                                {
    354                                         const btSoftBody::AJoint*       pja=(const btSoftBody::AJoint*)pj;
    355                                         const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
    356                                         const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
    357                                         const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
    358                                         const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
    359                                         idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
    360                                         idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
    361                                         idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
    362                                         idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
     354                                const btSoftBody::AJoint*       pja=(const btSoftBody::AJoint*)pj;
     355                                const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
     356                                const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
     357                                const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
     358                                const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
     359                                idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
     360                                idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
     361                                idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
     362                                idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
    363363                                }
    364364                        }               
    … …  
    369369//
    370370void                    btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
    371                                                                                         btIDebugDraw* idraw,
    372                                                                                         bool masses,
    373                                                                                         bool areas,
    374                                                                                         bool /*stress*/)
     371                                                  btIDebugDraw* idraw,
     372                                                  bool masses,
     373                                                  bool areas,
     374                                                  bool /*stress*/)
    375375{
    376376        for(int i=0;i<psb->m_nodes.size();++i)
    … …  
    395395//
    396396void                    btSoftBodyHelpers::DrawNodeTree(        btSoftBody* psb,
    397                                                                                                 btIDebugDraw* idraw,
    398                                                                                                 int mindepth,
    399                                                                                                 int maxdepth)
    400 {
    401         drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
     397                                                                                                        btIDebugDraw* idraw,
     398                                                                                                        int mindepth,
     399                                                                                                        int maxdepth)
     400{
     401drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
    402402}
    403403
    404404//
    405405void                    btSoftBodyHelpers::DrawFaceTree(        btSoftBody* psb,
    406                                                                                                 btIDebugDraw* idraw,
    407                                                                                                 int mindepth,
    408                                                                                                 int maxdepth)
    409 {
    410         drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
     406                                                                                                        btIDebugDraw* idraw,
     407                                                                                                        int mindepth,
     408                                                                                                        int maxdepth)
     409{
     410drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
    411411}
    412412
    413413//
    414414void                    btSoftBodyHelpers::DrawClusterTree(     btSoftBody* psb,
    415                                                                                                    btIDebugDraw* idraw,
    416                                                                                                    int mindepth,
    417                                                                                                    int maxdepth)
    418 {
    419         drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
     415                                                                                                        btIDebugDraw* idraw,
     416                                                                                                        int mindepth,
     417                                                                                                        int maxdepth)
     418{
     419drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
    420420}
    421421
    422422//
    423423void                    btSoftBodyHelpers::DrawFrame(           btSoftBody* psb,
    424                                                                                         btIDebugDraw* idraw)
     424                                                  btIDebugDraw* idraw)
    425425{
    426426        if(psb->m_pose.m_bframe)
    … …  
    446446//
    447447btSoftBody*             btSoftBodyHelpers::CreateRope(  btSoftBodyWorldInfo& worldInfo, const btVector3& from,
    448                                                                                           const btVector3& to,
    449                                                                                           int res,
    450                                                                                           int fixeds)
     448                                                   const btVector3& to,
     449                                                   int res,
     450                                                   int fixeds)
    451451{
    452452        /* Create nodes */
    … …  
    478478//
    479479btSoftBody*             btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
    480                                                                                            const btVector3& corner10,
    481                                                                                            const btVector3& corner01,
    482                                                                                            const btVector3& corner11,
    483                                                                                            int resx,
    484                                                                                            int resy,
    485                                                                                            int fixeds,
    486                                                                                            bool gendiags)
     480                                                        const btVector3& corner10,
     481                                                        const btVector3& corner01,
     482                                                        const btVector3& corner11,
     483                                                        int resx,
     484                                                        int resy,
     485                                                        int fixeds,
     486                                                        bool gendiags)
    487487{
    488488#define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
    … …  
    554554
    555555//
    556 btSoftBody*             btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
    557                                                                                                  const btVector3& corner00,
    558                                                                                                  const btVector3& corner10,
    559                                                                                                  const btVector3& corner01,
    560                                                                                                  const btVector3& corner11,
    561                                                                                                  int resx,
    562                                                                                                  int resy,
    563                                                                                                  int fixeds,
    564                                                                                                  bool gendiags,
    565                                                                                                  float* tex_coords)
    566 {
    567 
    568         /*
    569         *
    570         *  corners:
    571         *
    572         *  [0][0]     corner00 ------- corner01   [resx][0]
    573         *                |                |
    574         *                |                |
    575         *  [0][resy]  corner10 -------- corner11  [resx][resy]
    576         *
    577         *
    578         *
    579         *
    580         *
    581         *
    582         *   "fixedgs" map:
    583         *
    584         *  corner00     -->   +1
    585         *  corner01     -->   +2
    586         *  corner10     -->   +4
    587         *  corner11     -->   +8
    588         *  upper middle -->  +16
    589         *  left middle  -->  +32
    590         *  right middle -->  +64
    591         *  lower middle --> +128
    592         *  center       --> +256
    593         *
    594         *
    595         *   tex_coords size   (resx-1)*(resy-1)*12
    596         *
    597         *
    598         *
    599         *     SINGLE QUAD INTERNALS
    600         *
    601         *  1) btSoftBody's nodes and links,
    602         *     diagonal link is optional ("gendiags")
    603         *
    604         *
    605         *    node00 ------ node01
    606         *      | .             
    607         *      |   .           
    608         *      |     .         
    609         *      |       .       
    610         *      |         .     
    611         *    node10        node11
    612         *
    613         *
    614         *
    615         *   2) Faces:
    616         *      two triangles,
    617         *      UV Coordinates (hier example for single quad)
    618         *     
    619         *     (0,1)          (0,1)  (1,1)
    620         *     1 |\            3 \-----| 2
    621         *       | \              \    |
    622         *       |  \              \   |
    623         *       |   \              \  |
    624         *       |    \              \ |
    625         *     2 |-----\ 3            \| 1
    626         *     (0,0)    (1,0)       (1,0)
    627         *
    628         *
    629         *
    630         *
    631         *
    632         *
    633         */
    634 
    635 #define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
    636         /* Create nodes         */
    637         if((resx<2)||(resy<2)) return(0);
    638         const int       rx=resx;
    639         const int       ry=resy;
    640         const int       tot=rx*ry;
    641         btVector3*      x=new btVector3[tot];
    642         btScalar*       m=new btScalar[tot];
    643 
    644         for(int iy=0;iy<ry;++iy)
    645         {
    646                 const btScalar  ty=iy/(btScalar)(ry-1);
    647                 const btVector3 py0=lerp(corner00,corner01,ty);
    648                 const btVector3 py1=lerp(corner10,corner11,ty);
    649                 for(int ix=0;ix<rx;++ix)
    650                 {
    651                         const btScalar  tx=ix/(btScalar)(rx-1);
    652                         x[IDX(ix,iy)]=lerp(py0,py1,tx);
    653                         m[IDX(ix,iy)]=1;
    654                 }
    655         }
    656         btSoftBody*     psb=new btSoftBody(&worldInfo,tot,x,m);
    657         if(fixeds&1)            psb->setMass(IDX(0,0),0);
    658         if(fixeds&2)            psb->setMass(IDX(rx-1,0),0);
    659         if(fixeds&4)            psb->setMass(IDX(0,ry-1),0);
    660         if(fixeds&8)            psb->setMass(IDX(rx-1,ry-1),0);
    661         if(fixeds&16)           psb->setMass(IDX((rx-1)/2,0),0);
    662         if(fixeds&32)           psb->setMass(IDX(0,(ry-1)/2),0);
    663         if(fixeds&64)           psb->setMass(IDX(rx-1,(ry-1)/2),0);
    664         if(fixeds&128)          psb->setMass(IDX((rx-1)/2,ry-1),0);
    665         if(fixeds&256)          psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
    666         delete[] x;
    667         delete[] m;
    668 
    669 
    670         int z = 0;
    671         /* Create links and faces       */
    672         for(int iy=0;iy<ry;++iy)
    673         {
    674                 for(int ix=0;ix<rx;++ix)
    675                 {
    676                         const bool      mdx=(ix+1)<rx;
    677                         const bool      mdy=(iy+1)<ry;
    678 
    679                         int node00=IDX(ix,iy);
    680                         int node01=IDX(ix+1,iy);
    681                         int node10=IDX(ix,iy+1);
    682                         int node11=IDX(ix+1,iy+1);
    683 
    684                         if(mdx) psb->appendLink(node00,node01);
    685                         if(mdy) psb->appendLink(node00,node10);
    686                         if(mdx&&mdy)
    687                         {
    688                                 psb->appendFace(node00,node10,node11);
    689                                 if (tex_coords) {
    690                                         tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
    691                                         tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
    692                                         tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
    693                                         tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
    694                                         tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
    695                                         tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
    696                                 }
    697                                 psb->appendFace(node11,node01,node00);
    698                                 if (tex_coords) {
    699                                         tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
    700                                         tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
    701                                         tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
    702                                         tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
    703                                         tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
    704                                         tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
    705                                 }
    706                                 if (gendiags) psb->appendLink(node00,node11);
    707                                 z += 12;
    708                         }
    709                 }
    710         }
    711         /* Finished     */
    712 #undef IDX
    713         return(psb);
    714 }
    715 
    716 float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
    717 {
    718 
    719         /*
    720         *
    721         *
    722         *    node00 --- node01
    723         *      |          |
    724         *    node10 --- node11
    725         *
    726         *
    727         *   ID map:
    728         *
    729         *   node00 s --> 0
    730         *   node00 t --> 1
    731         *
    732         *   node01 s --> 3
    733         *   node01 t --> 1
    734         *
    735         *   node10 s --> 0
    736         *   node10 t --> 2
    737         *
    738         *   node11 s --> 3
    739         *   node11 t --> 2
    740         *
    741         *
    742         */
    743 
    744         float tc=0.0f;
    745         if (id == 0) {
    746                 tc = (1.0f/((resx-1))*ix);
    747         }
    748         else if (id==1) {
    749                 tc = (1.0f/((resy-1))*(resy-1-iy));
    750         }
    751         else if (id==2) {
    752                 tc = (1.0f/((resy-1))*(resy-1-iy-1));
    753         }
    754         else if (id==3) {
    755                 tc = (1.0f/((resx-1))*(ix+1));
    756         }
    757         return tc;
    758 }
    759 //
    760556btSoftBody*             btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
    761                                                                                                    const btVector3& radius,
    762                                                                                                    int res)
     557                                                                const btVector3& radius,
     558                                                                int res)
    763559{
    764560        struct  Hammersley
    … …  
    791587//
    792588btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
    793                                                                                                         const int* triangles,
    794                                                                                                         int ntriangles)
     589                                                                  const int* triangles,
     590                                                                  int ntriangles)
    795591{
    796592        int             maxidx=0;
    … …  
    833629//
    834630btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
    835                                                                                                                 int nvertices)
     631                                                                         int nvertices)
    836632{
    837633        HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.h

    r1963 r1972  
    4747        /* Draw body                                                                                                                    */
    4848        static void                             Draw(           btSoftBody* psb,
    49                 btIDebugDraw* idraw,
    50                 int drawflags=fDrawFlags::Std);
     49                                                                                btIDebugDraw* idraw,
     50                                                                                int drawflags=fDrawFlags::Std);
    5151        /* Draw body infos                                                                                                              */
    5252        static  void                    DrawInfos(      btSoftBody* psb,
    53                 btIDebugDraw* idraw,
    54                 bool masses,
    55                 bool areas,
    56                 bool stress);
     53                                                                                btIDebugDraw* idraw,
     54                                                                                bool masses,
     55                                                                                bool areas,
     56                                                                                bool stress);
    5757        /* Draw node tree                                                                                                               */
    5858        static void                             DrawNodeTree(   btSoftBody* psb,
    59                 btIDebugDraw* idraw,
    60                 int mindepth=0,
    61                 int maxdepth=-1);
     59                                                                                        btIDebugDraw* idraw,
     60                                                                                        int mindepth=0,
     61                                                                                        int maxdepth=-1);
    6262        /* Draw face tree                                                                                                               */
    6363        static void                             DrawFaceTree(   btSoftBody* psb,
    64                 btIDebugDraw* idraw,
    65                 int mindepth=0,
    66                 int maxdepth=-1);
     64                                                                                        btIDebugDraw* idraw,
     65                                                                                        int mindepth=0,
     66                                                                                        int maxdepth=-1);
    6767        /* Draw cluster tree                                                                                                    */
    6868        static void                             DrawClusterTree(btSoftBody* psb,
    69                 btIDebugDraw* idraw,
    70                 int mindepth=0,
    71                 int maxdepth=-1);
     69                                                                                        btIDebugDraw* idraw,
     70                                                                                        int mindepth=0,
     71                                                                                        int maxdepth=-1);
    7272        /* Draw rigid frame                                                                                                             */
    7373        static  void                    DrawFrame(              btSoftBody* psb,
    74                 btIDebugDraw* idraw);
     74                                                                                        btIDebugDraw* idraw);
    7575        /* Create a rope                                                                                                                */
    7676        static  btSoftBody*             CreateRope( btSoftBodyWorldInfo& worldInfo,
    77                 const btVector3& from,
    78                 const btVector3& to,
    79                 int res,
    80                 int fixeds);
     77                                                                                const btVector3& from,
     78                                                                                const btVector3& to,
     79                                                                                int res,
     80                                                                                int fixeds);
    8181        /* Create a patch                                                                                                               */
    8282        static  btSoftBody*             CreatePatch(btSoftBodyWorldInfo& worldInfo,
    83                 const btVector3& corner00,
    84                 const btVector3& corner10,
    85                 const btVector3& corner01,
    86                 const btVector3& corner11,
    87                 int resx,
    88                 int resy,
    89                 int fixeds,
    90                 bool gendiags);
    91         /* Create a patch with UV Texture Coordinates   */
    92         static  btSoftBody*             CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
    93                 const btVector3& corner00,
    94                 const btVector3& corner10,
    95                 const btVector3& corner01,
    96                 const btVector3& corner11,
    97                 int resx,
    98                 int resy,
    99                 int fixeds,
    100                 bool gendiags,
    101                 float* tex_coords=0);
    102         static  float   CalculateUV(int resx,int resy,int ix,int iy,int id);
     83                                                                                const btVector3& corner00,
     84                                                                                const btVector3& corner10,
     85                                                                                const btVector3& corner01,
     86                                                                                const btVector3& corner11,
     87                                                                                int resx,
     88                                                                                int resy,
     89                                                                                int fixeds,
     90                                                                                bool gendiags);
    10391        /* Create an ellipsoid                                                                                                  */
    10492        static  btSoftBody*             CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
    105                 const btVector3& center,
    106                 const btVector3& radius,
    107                 int res);       
     93                                                                                        const btVector3& center,
     94                                                                                        const btVector3& radius,
     95                                                                                        int res);       
    10896        /* Create from trimesh                                                                                                  */
    10997        static  btSoftBody*             CreateFromTriMesh(      btSoftBodyWorldInfo& worldInfo,
    110                 const btScalar* vertices,
    111                 const int* triangles,
    112                 int ntriangles);
     98                                                                                                const btScalar* vertices,
     99                                                                                                const int* triangles,
     100                                                                                                int ntriangles);
    113101        /* Create from convex-hull                                                                                              */
    114102        static  btSoftBody*             CreateFromConvexHull(   btSoftBodyWorldInfo& worldInfo,
    115                 const btVector3* vertices,
    116                 int nvertices);
     103                                                                                                        const btVector3* vertices,
     104                                                                                                        int nvertices);
    117105};
    118106

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h

    r1963 r1972  
    3232struct btSymMatrix
    3333{
    34         btSymMatrix() : dim(0)                                  {}
    35         btSymMatrix(int n,const T& init=T())    { resize(n,init); }
    36         void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
    37         int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
    38         T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
    39         const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
    40         btAlignedObjectArray<T> store;
    41         int                                             dim;
     34                                                btSymMatrix() : dim(0)                                  {}
     35                                                btSymMatrix(int n,const T& init=T())    { resize(n,init); }
     36void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
     37int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
     38T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
     39const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
     40btAlignedObjectArray<T> store;
     41int                                             dim;
    4242};     
    4343
    … …  
    4949public:
    5050        btSoftBody*                                             m_body;
    51 
     51       
    5252        btSoftBodyCollisionShape(btSoftBody* backptr)
    5353        {
    … …  
    7070        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
    7171        {
    72                 /* t should be identity, but better be safe than...fast? */
    73                 const btVector3 mins=m_body->m_bounds[0];
    74                 const btVector3 maxs=m_body->m_bounds[1];
    75                 const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
    76                         t*btVector3(maxs.x(),mins.y(),mins.z()),
    77                         t*btVector3(maxs.x(),maxs.y(),mins.z()),
    78                         t*btVector3(mins.x(),maxs.y(),mins.z()),
    79                         t*btVector3(mins.x(),mins.y(),maxs.z()),
    80                         t*btVector3(maxs.x(),mins.y(),maxs.z()),
    81                         t*btVector3(maxs.x(),maxs.y(),maxs.z()),
    82                         t*btVector3(mins.x(),maxs.y(),maxs.z())};
    83                 aabbMin=aabbMax=crns[0];
    84                 for(int i=1;i<8;++i)
    85                 {
    86                         aabbMin.setMin(crns[i]);
    87                         aabbMax.setMax(crns[i]);
    88                 }
    89         }
    90 
    91 
     72        /* t should be identity, but better be safe than...fast? */
     73        const btVector3 mins=m_body->m_bounds[0];
     74        const btVector3 maxs=m_body->m_bounds[1];
     75        const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
     76                                                        t*btVector3(maxs.x(),mins.y(),mins.z()),
     77                                                        t*btVector3(maxs.x(),maxs.y(),mins.z()),
     78                                                        t*btVector3(mins.x(),maxs.y(),mins.z()),
     79                                                        t*btVector3(mins.x(),mins.y(),maxs.z()),
     80                                                        t*btVector3(maxs.x(),mins.y(),maxs.z()),
     81                                                        t*btVector3(maxs.x(),maxs.y(),maxs.z()),
     82                                                        t*btVector3(mins.x(),maxs.y(),maxs.z())};
     83        aabbMin=aabbMax=crns[0];
     84        for(int i=1;i<8;++i)
     85                {
     86                aabbMin.setMin(crns[i]);
     87                aabbMax.setMax(crns[i]);
     88                }
     89        }
     90
     91       
    9292        virtual void    setLocalScaling(const btVector3& /*scaling*/)
    9393        {               
    … …  
    9696        virtual const btVector3& getLocalScaling() const
    9797        {
    98                 static const btVector3 dummy(1,1,1);
    99                 return dummy;
     98        static const btVector3 dummy(1,1,1);
     99        return dummy;
    100100        }
    101101        virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
    … …  
    120120
    121121        btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
    122 
    123 
     122       
     123       
    124124        virtual btVector3       localGetSupportingVertex(const btVector3& vec) const
    125         {
     125                {
    126126                btSoftBody::Node* const *                                               n=&m_cluster->m_nodes[0];
    127127                btScalar                                                                                d=dot(vec,n[0]->m_x);
    128128                int                                                                                             j=0;
    129129                for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
    130                 {
     130                        {
    131131                        const btScalar  k=dot(vec,n[i]->m_x);
    132132                        if(k>d) { d=k;j=i; }
    133                 }
     133                        }
    134134                return(n[j]->m_x);
    135         }
     135                }
    136136        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const
    137         {
     137                {
    138138                return(localGetSupportingVertex(vec));
    139         }
     139                }
    140140        //notice that the vectors should be unit length
    141141        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
    … …  
    150150
    151151        virtual int     getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
    152 
     152       
    153153        //debugging
    154154        virtual const char*     getName()const {return "SOFTCLUSTER";}
    … …  
    172172static inline void                      ZeroInitialize(T& value)
    173173{
    174         static const T  zerodummy;
    175         value=zerodummy;
     174static const T  zerodummy;
     175value=zerodummy;
    176176}
    177177//
    … …  
    193193//
    194194static inline btMatrix3x3       Lerp(   const btMatrix3x3& a,
    195                                                                  const btMatrix3x3& b,
    196                                                                  btScalar t)
    197 {
    198         btMatrix3x3     r;
    199         r[0]=Lerp(a[0],b[0],t);
    200         r[1]=Lerp(a[1],b[1],t);
    201         r[2]=Lerp(a[2],b[2],t);
    202         return(r);
     195                                                                        const btMatrix3x3& b,
     196                                                                        btScalar t)
     197{
     198btMatrix3x3     r;
     199r[0]=Lerp(a[0],b[0],t);
     200r[1]=Lerp(a[1],b[1],t);
     201r[2]=Lerp(a[2],b[2],t);
     202return(r);
    203203}
    204204//
    205205static inline btVector3         Clamp(const btVector3& v,btScalar maxlength)
    206206{
    207         const btScalar sql=v.length2();
    208         if(sql>(maxlength*maxlength))
    209                 return((v*maxlength)/btSqrt(sql));
     207const btScalar sql=v.length2();
     208if(sql>(maxlength*maxlength))
     209        return((v*maxlength)/btSqrt(sql));
    210210        else
    211                 return(v);
     211        return(v);
    212212}
    213213//
    … …  
    234234static inline btScalar          ClusterMetric(const btVector3& x,const btVector3& y)
    235235{
    236         const btVector3 d=x-y;
    237         return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
     236const btVector3 d=x-y;
     237return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
    238238}
    239239//
    … …  
    272272//
    273273static inline btMatrix3x3       Add(const btMatrix3x3& a,
    274                                                                 const btMatrix3x3& b)
     274        const btMatrix3x3& b)
    275275{
    276276        btMatrix3x3     r;
    … …  
    280280//
    281281static inline btMatrix3x3       Sub(const btMatrix3x3& a,
    282                                                                 const btMatrix3x3& b)
     282        const btMatrix3x3& b)
    283283{
    284284        btMatrix3x3     r;
    … …  
    288288//
    289289static inline btMatrix3x3       Mul(const btMatrix3x3& a,
    290                                                                 btScalar b)
     290        btScalar b)
    291291{
    292292        btMatrix3x3     r;
    … …  
    297297static inline void                      Orthogonalize(btMatrix3x3& m)
    298298{
    299         m[2]=cross(m[0],m[1]).normalized();
    300         m[1]=cross(m[2],m[0]).normalized();
    301         m[0]=cross(m[1],m[2]).normalized();
     299m[2]=cross(m[0],m[1]).normalized();
     300m[1]=cross(m[2],m[0]).normalized();
     301m[0]=cross(m[1],m[2]).normalized();
    302302}
    303303//
    … …  
    310310//
    311311static inline btMatrix3x3       ImpulseMatrix(  btScalar dt,
    312                                                                                   btScalar ima,
    313                                                                                   btScalar imb,
    314                                                                                   const btMatrix3x3& iwi,
    315                                                                                   const btVector3& r)
     312        btScalar ima,
     313        btScalar imb,
     314        const btMatrix3x3& iwi,
     315        const btVector3& r)
    316316{
    317317        return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
    … …  
    320320//
    321321static inline btMatrix3x3       ImpulseMatrix(  btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
    322                                                                                   btScalar imb,const btMatrix3x3& iib,const btVector3& rb)     
    323 {
    324         return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
     322                                                                                        btScalar imb,const btMatrix3x3& iib,const btVector3& rb)       
     323{
     324return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
    325325}
    326326
    327327//
    328328static inline btMatrix3x3       AngularImpulseMatrix(   const btMatrix3x3& iia,
    329                                                                                                  const btMatrix3x3& iib)
    330 {
    331         return(Add(iia,iib).inverse());
     329                                                                                                        const btMatrix3x3& iib)
     330{
     331return(Add(iia,iib).inverse());
    332332}
    333333
    334334//
    335335static inline btVector3         ProjectOnAxis(  const btVector3& v,
    336                                                                                   const btVector3& a)
     336        const btVector3& a)
    337337{
    338338        return(a*dot(v,a));
    … …  
    340340//
    341341static inline btVector3         ProjectOnPlane( const btVector3& v,
    342                                                                                    const btVector3& a)
     342        const btVector3& a)
    343343{
    344344        return(v-ProjectOnAxis(v,a));
    … …  
    347347//
    348348static inline void                      ProjectOrigin(  const btVector3& a,
    349                                                                                   const btVector3& b,
    350                                                                                   btVector3& prj,
    351                                                                                   btScalar& sqd)
    352 {
    353         const btVector3 d=b-a;
    354         const btScalar  m2=d.length2();
    355         if(m2>SIMD_EPSILON)
     349                                                                                        const btVector3& b,
     350                                                                                        btVector3& prj,
     351                                                                                        btScalar& sqd)
     352{
     353const btVector3 d=b-a;
     354const btScalar  m2=d.length2();
     355if(m2>SIMD_EPSILON)
    356356        {       
    357                 const btScalar  t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
    358                 const btVector3 p=a+d*t;
    359                 const btScalar  l2=p.length2();
    360                 if(l2<sqd)
    361                 {
     357        const btScalar  t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
     358        const btVector3 p=a+d*t;
     359        const btScalar  l2=p.length2();
     360        if(l2<sqd)
     361                {
     362                prj=p;
     363                sqd=l2;
     364                }
     365        }
     366}
     367//
     368static inline void                      ProjectOrigin(  const btVector3& a,
     369                                                                                        const btVector3& b,
     370                                                                                        const btVector3& c,
     371                                                                                        btVector3& prj,
     372                                                                                        btScalar& sqd)
     373{
     374const btVector3&        q=cross(b-a,c-a);
     375const btScalar          m2=q.length2();
     376if(m2>SIMD_EPSILON)
     377        {
     378        const btVector3 n=q/btSqrt(m2);
     379        const btScalar  k=dot(a,n);
     380        const btScalar  k2=k*k;
     381        if(k2<sqd)
     382                {
     383                const btVector3 p=n*k;
     384                if(     (dot(cross(a-p,b-p),q)>0)&&
     385                        (dot(cross(b-p,c-p),q)>0)&&
     386                        (dot(cross(c-p,a-p),q)>0))
     387                        {                       
    362388                        prj=p;
    363                         sqd=l2;
    364                 }
    365         }
    366 }
    367 //
    368 static inline void                      ProjectOrigin(  const btVector3& a,
    369                                                                                   const btVector3& b,
    370                                                                                   const btVector3& c,
    371                                                                                   btVector3& prj,
    372                                                                                   btScalar& sqd)
    373 {
    374         const btVector3&        q=cross(b-a,c-a);
    375         const btScalar          m2=q.length2();
    376         if(m2>SIMD_EPSILON)
    377         {
    378                 const btVector3 n=q/btSqrt(m2);
    379                 const btScalar  k=dot(a,n);
    380                 const btScalar  k2=k*k;
    381                 if(k2<sqd)
    382                 {
    383                         const btVector3 p=n*k;
    384                         if(     (dot(cross(a-p,b-p),q)>0)&&
    385                                 (dot(cross(b-p,c-p),q)>0)&&
    386                                 (dot(cross(c-p,a-p),q)>0))
    387                         {                       
    388                                 prj=p;
    389                                 sqd=k2;
     389                        sqd=k2;
    390390                        }
    391391                        else
    392392                        {
    393                                 ProjectOrigin(a,b,prj,sqd);
    394                                 ProjectOrigin(b,c,prj,sqd);
    395                                 ProjectOrigin(c,a,prj,sqd);
     393                        ProjectOrigin(a,b,prj,sqd);
     394                        ProjectOrigin(b,c,prj,sqd);
     395                        ProjectOrigin(c,a,prj,sqd);
    396396                        }
    397397                }
    … …  
    402402template <typename T>
    403403static inline T                         BaryEval(               const T& a,
    404                                                                          const T& b,
    405                                                                          const T& c,
    406                                                                          const btVector3& coord)
     404                                                                                        const T& b,
     405                                                                                        const T& c,
     406                                                                                        const btVector3& coord)
    407407{
    408408        return(a*coord.x()+b*coord.y()+c*coord.z());
    … …  
    410410//
    411411static inline btVector3         BaryCoord(      const btVector3& a,
    412                                                                           const btVector3& b,
    413                                                                           const btVector3& c,
    414                                                                           const btVector3& p)
    415 {
    416         const btScalar  w[]={   cross(a-p,b-p).length(),
    417                 cross(b-p,c-p).length(),
    418                 cross(c-p,a-p).length()};
    419         const btScalar  isum=1/(w[0]+w[1]+w[2]);
    420         return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
     412                                                                                const btVector3& b,
     413                                                                                const btVector3& c,
     414                                                                                const btVector3& p)
     415{
     416const btScalar  w[]={   cross(a-p,b-p).length(),
     417                                                cross(b-p,c-p).length(),
     418                                                cross(c-p,a-p).length()};
     419const btScalar  isum=1/(w[0]+w[1]+w[2]);
     420return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
    421421}
    422422
    423423//
    424424static btScalar                         ImplicitSolve(  btSoftBody::ImplicitFn* fn,
    425                                                                                   const btVector3& a,
    426                                                                                   const btVector3& b,
    427                                                                                   const btScalar accuracy,
    428                                                                                   const int maxiterations=256)
    429 {
    430         btScalar        span[2]={0,1};
    431         btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
    432         if(values[0]>values[1])
    433         {
    434                 btSwap(span[0],span[1]);
    435                 btSwap(values[0],values[1]);
    436         }
    437         if(values[0]>-accuracy) return(-1);
    438         if(values[1]<+accuracy) return(-1);
    439         for(int i=0;i<maxiterations;++i)
    440         {
    441                 const btScalar  t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
    442                 const btScalar  v=fn->Eval(Lerp(a,b,t));
    443                 if((t<=0)||(t>=1))              break;
    444                 if(btFabs(v)<accuracy)  return(t);
    445                 if(v<0)
     425                                                                                        const btVector3& a,
     426                                                                                        const btVector3& b,
     427                                                                                        const btScalar accuracy,
     428                                                                                        const int maxiterations=256)
     429{
     430btScalar        span[2]={0,1};
     431btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
     432if(values[0]>values[1])
     433        {
     434        btSwap(span[0],span[1]);
     435        btSwap(values[0],values[1]);
     436        }
     437if(values[0]>-accuracy) return(-1);
     438if(values[1]<+accuracy) return(-1);
     439for(int i=0;i<maxiterations;++i)
     440        {
     441        const btScalar  t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
     442        const btScalar  v=fn->Eval(Lerp(a,b,t));
     443        if((t<=0)||(t>=1))              break;
     444        if(btFabs(v)<accuracy)  return(t);
     445        if(v<0)
    446446                { span[0]=t;values[0]=v; }
    447447                else
    448448                { span[1]=t;values[1]=v; }
    449449        }
    450         return(-1);
     450return(-1);
    451451}
    452452
    … …  
    463463//
    464464static inline btDbvtVolume      VolumeOf(       const btSoftBody::Face& f,
    465                                                                          btScalar margin)
    466 {
    467         const btVector3*        pts[]={ &f.m_n[0]->m_x,
    468                 &f.m_n[1]->m_x,
    469                 &f.m_n[2]->m_x};
    470         btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
    471         vol.Expand(btVector3(margin,margin,margin));
    472         return(vol);
     465                                                                                btScalar margin)
     466{
     467const btVector3*        pts[]={ &f.m_n[0]->m_x,
     468                                                        &f.m_n[1]->m_x,
     469                                                        &f.m_n[2]->m_x};
     470btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
     471vol.Expand(btVector3(margin,margin,margin));
     472return(vol);
    473473}
    474474
    … …  
    476476static inline btVector3                 CenterOf(       const btSoftBody::Face& f)
    477477{
    478         return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
     478return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
    479479}
    480480
    481481//
    482482static inline btScalar                  AreaOf(         const btVector3& x0,
    483                                                                            const btVector3& x1,
    484                                                                            const btVector3& x2)
     483                                                                                        const btVector3& x1,
     484                                                                                        const btVector3& x2)
    485485{
    486486        const btVector3 a=x1-x0;
    … …  
    493493//
    494494static inline btScalar          VolumeOf(       const btVector3& x0,
    495                                                                          const btVector3& x1,
    496                                                                          const btVector3& x2,
    497                                                                          const btVector3& x3)
     495                                                                                const btVector3& x1,
     496                                                                                const btVector3& x2,
     497                                                                                const btVector3& x3)
    498498{
    499499        const btVector3 a=x1-x0;
    … …  
    505505//
    506506static void                                     EvaluateMedium( const btSoftBodyWorldInfo* wfi,
    507                                                                                    const btVector3& x,
    508                                                                                    btSoftBody::sMedium& medium)
     507                                                                                        const btVector3& x,
     508                                                                                        btSoftBody::sMedium& medium)
    509509{
    510510        medium.m_velocity       =       btVector3(0,0,0);
    … …  
    524524//
    525525static inline void                      ApplyClampedForce(      btSoftBody::Node& n,
    526                                                                                           const btVector3& f,
    527                                                                                           btScalar dt)
     526                                                                                                const btVector3& f,
     527                                                                                                btScalar dt)
    528528{
    529529        const btScalar  dtim=dt*n.m_im;
    … …  
    540540//
    541541static inline int               MatchEdge(      const btSoftBody::Node* a,
    542                                                                   const btSoftBody::Node* b,
    543                                                                   const btSoftBody::Node* ma,
    544                                                                   const btSoftBody::Node* mb)
    545 {
    546         if((a==ma)&&(b==mb)) return(0);
    547         if((a==mb)&&(b==ma)) return(1);
    548         return(-1);
     542                                                                        const btSoftBody::Node* b,
     543                                                                        const btSoftBody::Node* ma,
     544                                                                        const btSoftBody::Node* mb)
     545{
     546if((a==ma)&&(b==mb)) return(0);
     547if((a==mb)&&(b==ma)) return(1);
     548return(-1);
    549549}
    550550
    … …  
    556556struct  btEigen
    557557{
    558         static int                      system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
    559         {
    560                 static const int                maxiterations=16;
    561                 static const btScalar   accuracy=(btScalar)0.0001;
    562                 btMatrix3x3&                    v=*vectors;
    563                 int                                             iterations=0;
    564                 vectors->setIdentity();
    565                 do      {
    566                         int                             p=0,q=1;
    567                         if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
    568                         if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
    569                         if(btFabs(a[p][q])>accuracy)
     558static int                      system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
     559        {
     560        static const int                maxiterations=16;
     561        static const btScalar   accuracy=(btScalar)0.0001;
     562        btMatrix3x3&                    v=*vectors;
     563        int                                             iterations=0;
     564        vectors->setIdentity();
     565        do      {
     566                int                             p=0,q=1;
     567                if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
     568                if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
     569                if(btFabs(a[p][q])>accuracy)
    570570                        {
    571                                 const btScalar  w=(a[q][q]-a[p][p])/(2*a[p][q]);
    572                                 const btScalar  z=btFabs(w);
    573                                 const btScalar  t=w/(z*(btSqrt(1+w*w)+z));
    574                                 if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
     571                        const btScalar  w=(a[q][q]-a[p][p])/(2*a[p][q]);
     572                        const btScalar  z=btFabs(w);
     573                        const btScalar  t=w/(z*(btSqrt(1+w*w)+z));
     574                        if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
    575575                                {
    576                                         const btScalar  c=1/btSqrt(t*t+1);
    577                                         const btScalar  s=c*t;
    578                                         mulPQ(a,c,s,p,q);
    579                                         mulTPQ(a,c,s,p,q);
    580                                         mulPQ(v,c,s,p,q);
     576                                const btScalar  c=1/btSqrt(t*t+1);
     577                                const btScalar  s=c*t;
     578                                mulPQ(a,c,s,p,q);
     579                                mulTPQ(a,c,s,p,q);
     580                                mulPQ(v,c,s,p,q);
    581581                                } else break;
    582582                        } else break;
    583583                } while((++iterations)<maxiterations);
    584                 if(values)
    585                 {
    586                         *values=btVector3(a[0][0],a[1][1],a[2][2]);
    587                 }
    588                 return(iterations);
     584        if(values)
     585                {
     586                *values=btVector3(a[0][0],a[1][1],a[2][2]);
     587                }
     588        return(iterations);
    589589        }
    590590private:
    591         static inline void      mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
    592         {
    593                 const btScalar  m[2][3]={       {a[p][0],a[p][1],a[p][2]},
    594                 {a[q][0],a[q][1],a[q][2]}};
    595                 int i;
    596 
    597                 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
    598                 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
    599         }
    600         static inline void      mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
    601         {
    602                 const btScalar  m[2][3]={       {a[0][p],a[1][p],a[2][p]},
    603                 {a[0][q],a[1][q],a[2][q]}};
    604                 int i;
    605 
    606                 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
    607                 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
     591static inline void      mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
     592        {
     593        const btScalar  m[2][3]={       {a[p][0],a[p][1],a[p][2]},
     594                                                                {a[q][0],a[q][1],a[q][2]}};
     595        int i;
     596
     597        for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
     598        for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
     599        }
     600static inline void      mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
     601        {
     602        const btScalar  m[2][3]={       {a[0][p],a[1][p],a[2][p]},
     603                                                                {a[0][q],a[1][q],a[2][q]}};
     604        int i;
     605
     606        for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
     607        for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
    608608        }
    609609};
    … …  
    640640                s.setIdentity();
    641641        }
    642         return(i);
     642return(i);
    643643}
    644644
    … …  
    653653        struct  ClusterBase : btDbvt::ICollide
    654654        {
    655                 btScalar                        erp;
    656                 btScalar                        idt;
    657                 btScalar                        margin;
    658                 btScalar                        friction;
    659                 btScalar                        threshold;
    660                 ClusterBase()
    661                 {
    662                         erp                     =(btScalar)1;
    663                         idt                     =0;
    664                         margin          =0;
    665                         friction        =0;
    666                         threshold       =(btScalar)0;
    667                 }
    668                 bool                            SolveContact(   const btGjkEpaSolver2::sResults& res,
    669                         btSoftBody::Body ba,btSoftBody::Body bb,
    670                         btSoftBody::CJoint& joint)
    671                 {
    672                         if(res.distance<margin)
     655        btScalar                        erp;
     656        btScalar                        idt;
     657        btScalar                        margin;
     658        btScalar                        friction;
     659        btScalar                        threshold;
     660                                        ClusterBase()
     661                {
     662                erp                     =(btScalar)1;
     663                idt                     =0;
     664                margin          =0;
     665                friction        =0;
     666                threshold       =(btScalar)0;
     667                }
     668        bool                            SolveContact(   const btGjkEpaSolver2::sResults& res,
     669                                                                                btSoftBody::Body ba,btSoftBody::Body bb,
     670                                                                                btSoftBody::CJoint& joint)
     671                {
     672                if(res.distance<margin)
    673673                        {
    674                                 const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
    675                                 const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
    676                                 const btVector3         va=ba.velocity(ra);
    677                                 const btVector3         vb=bb.velocity(rb);
    678                                 const btVector3         vrel=va-vb;
    679                                 const btScalar          rvac=dot(vrel,res.normal);
    680                                 const btScalar          depth=res.distance-margin;
    681                                 const btVector3         iv=res.normal*rvac;
    682                                 const btVector3         fv=vrel-iv;
    683                                 joint.m_bodies[0]       =       ba;
    684                                 joint.m_bodies[1]       =       bb;
    685                                 joint.m_refs[0]         =       ra*ba.xform().getBasis();
    686                                 joint.m_refs[1]         =       rb*bb.xform().getBasis();
    687                                 joint.m_rpos[0]         =       ra;
    688                                 joint.m_rpos[1]         =       rb;
    689                                 joint.m_cfm                     =       1;
    690                                 joint.m_erp                     =       1;
    691                                 joint.m_life            =       0;
    692                                 joint.m_maxlife         =       0;
    693                                 joint.m_split           =       1;
    694                                 joint.m_drift           =       depth*res.normal;
    695                                 joint.m_normal          =       res.normal;
    696                                 joint.m_delete          =       false;
    697                                 joint.m_friction        =       fv.length2()<(-rvac*friction)?1:friction;
    698                                 joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
    699                                         bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
    700                                 return(true);
     674                        const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
     675                        const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
     676                        const btVector3         va=ba.velocity(ra);
     677                        const btVector3         vb=bb.velocity(rb);
     678                        const btVector3         vrel=va-vb;
     679                        const btScalar          rvac=dot(vrel,res.normal);
     680                        const btScalar          depth=res.distance-margin;
     681                        const btVector3         iv=res.normal*rvac;
     682                        const btVector3         fv=vrel-iv;
     683                        joint.m_bodies[0]       =       ba;
     684                        joint.m_bodies[1]       =       bb;
     685                        joint.m_refs[0]         =       ra*ba.xform().getBasis();
     686                        joint.m_refs[1]         =       rb*bb.xform().getBasis();
     687                        joint.m_rpos[0]         =       ra;
     688                        joint.m_rpos[1]         =       rb;
     689                        joint.m_cfm                     =       1;
     690                        joint.m_erp                     =       1;
     691                        joint.m_life            =       0;
     692                        joint.m_maxlife         =       0;
     693                        joint.m_split           =       1;
     694                        joint.m_drift           =       depth*res.normal;
     695                        joint.m_normal          =       res.normal;
     696                        joint.m_delete          =       false;
     697                        joint.m_friction        =       fv.length2()<(-rvac*friction)?1:friction;
     698                        joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
     699                                                                                                        bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
     700                        return(true);
    701701                        }
    702                         return(false);
     702                return(false);
    703703                }
    704704        };
    … …  
    708708        struct  CollideCL_RS : ClusterBase
    709709        {
    710                 btSoftBody*             psb;
    711                 btRigidBody*    prb;
    712                 void            Process(const btDbvtNode* leaf)
    713                 {
    714                         btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
    715                         btSoftClusterCollisionShape     cshape(cluster);
    716                         const btConvexShape*            rshape=(const btConvexShape*)prb->getCollisionShape();
    717                         btGjkEpaSolver2::sResults       res;           
    718                         if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
    719                                 rshape,prb->getInterpolationWorldTransform(),
    720                                 btVector3(1,0,0),res))
     710        btSoftBody*             psb;
     711        btRigidBody*    prb;
     712        void            Process(const btDbvtNode* leaf)
     713                {
     714                btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
     715                btSoftClusterCollisionShape     cshape(cluster);
     716                const btConvexShape*            rshape=(const btConvexShape*)prb->getCollisionShape();
     717                btGjkEpaSolver2::sResults       res;           
     718                if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
     719                                                                                        rshape,prb->getInterpolationWorldTransform(),
     720                                                                                        btVector3(1,0,0),res))
    721721                        {
    722                                 btSoftBody::CJoint      joint;
    723                                 if(SolveContact(res,cluster,prb,joint))
     722                        btSoftBody::CJoint      joint;
     723                        if(SolveContact(res,cluster,prb,joint))
    724724                                {
    725                                         btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
    726                                         *pj=joint;psb->m_joints.push_back(pj);
    727                                         if(prb->isStaticOrKinematicObject())
     725                                btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
     726                                *pj=joint;psb->m_joints.push_back(pj);
     727                                if(prb->isStaticOrKinematicObject())
    728728                                        {
    729                                                 pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
    730                                                 pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
     729                                        pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
     730                                        pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
    731731                                        }
    732732                                        else
    733733                                        {
    734                                                 pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
    735                                                 pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
     734                                        pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
     735                                        pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
    736736                                        }
    737737                                }
    738738                        }
    739739                }
    740                 void            Process(btSoftBody* ps,btRigidBody* pr)
    741                 {
    742                         psb                     =       ps;
    743                         prb                     =       pr;
    744                         idt                     =       ps->m_sst.isdt;
    745                         margin          =       ps->getCollisionShape()->getMargin()+
    746                                 pr->getCollisionShape()->getMargin();
    747                         friction        =       btMin(psb->m_cfg.kDF,prb->getFriction());
    748                         btVector3                       mins;
    749                         btVector3                       maxs;
    750 
    751                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
    752                         pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
    753                         volume=btDbvtVolume::FromMM(mins,maxs);
    754                         volume.Expand(btVector3(1,1,1)*margin);
    755                         btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
     740        void            Process(btSoftBody* ps,btRigidBody* pr)
     741                {
     742                psb                     =       ps;
     743                prb                     =       pr;
     744                idt                     =       ps->m_sst.isdt;
     745                margin          =       ps->getCollisionShape()->getMargin()+
     746                                                pr->getCollisionShape()->getMargin();
     747                friction        =       btMin(psb->m_cfg.kDF,prb->getFriction());
     748                btVector3                       mins;
     749                btVector3                       maxs;
     750               
     751                ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
     752                pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
     753                volume=btDbvtVolume::FromMM(mins,maxs);
     754                volume.Expand(btVector3(1,1,1)*margin);
     755                btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
    756756                }       
    757757        };
    … …  
    761761        struct  CollideCL_SS : ClusterBase
    762762        {
    763                 btSoftBody*     bodies[2];
    764                 void            Process(const btDbvtNode* la,const btDbvtNode* lb)
    765                 {
    766                         btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
    767                         btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
    768                         btSoftClusterCollisionShape     csa(cla);
    769                         btSoftClusterCollisionShape     csb(clb);
    770                         btGjkEpaSolver2::sResults       res;           
    771                         if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
    772                                 &csb,btTransform::getIdentity(),
    773                                 cla->m_com-clb->m_com,res))
     763        btSoftBody*     bodies[2];
     764        void            Process(const btDbvtNode* la,const btDbvtNode* lb)
     765                {
     766                btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
     767                btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
     768                btSoftClusterCollisionShape     csa(cla);
     769                btSoftClusterCollisionShape     csb(clb);
     770                btGjkEpaSolver2::sResults       res;           
     771                if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
     772                                                                                        &csb,btTransform::getIdentity(),
     773                                                                                        cla->m_com-clb->m_com,res))
    774774                        {
    775                                 btSoftBody::CJoint      joint;
    776                                 if(SolveContact(res,cla,clb,joint))
     775                        btSoftBody::CJoint      joint;
     776                        if(SolveContact(res,cla,clb,joint))
    777777                                {
    778                                         btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
    779                                         *pj=joint;bodies[0]->m_joints.push_back(pj);
    780                                         pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
    781                                         pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
     778                                btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
     779                                *pj=joint;bodies[0]->m_joints.push_back(pj);
     780                                pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
     781                                pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
    782782                                }
    783783                        }
    784784                }
    785                 void            Process(btSoftBody* psa,btSoftBody* psb)
    786                 {
    787                         idt                     =       psa->m_sst.isdt;
    788                         margin          =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
    789                         friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
    790                         bodies[0]       =       psa;
    791                         bodies[1]       =       psb;
    792                         btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
     785        void            Process(btSoftBody* psa,btSoftBody* psb)
     786                {
     787                idt                     =       psa->m_sst.isdt;
     788                margin          =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
     789                friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
     790                bodies[0]       =       psa;
     791                bodies[1]       =       psb;
     792                btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
    793793                }       
    794794        };
    … …  
    798798        struct  CollideSDF_RS : btDbvt::ICollide
    799799        {
    800                 void            Process(const btDbvtNode* leaf)
    801                 {
    802                         btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
    803                         DoNode(*node);
    804                 }
    805                 void            DoNode(btSoftBody::Node& n) const
    806                 {
    807                         const btScalar                  m=n.m_im>0?dynmargin:stamargin;
    808                         btSoftBody::RContact    c;
    809                         if(     (!n.m_battach)&&
    810                                 psb->checkContact(prb,n.m_x,m,c.m_cti))
     800        void            Process(const btDbvtNode* leaf)
     801                {
     802                btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
     803                DoNode(*node);
     804                }
     805        void            DoNode(btSoftBody::Node& n) const
     806                {
     807                const btScalar                  m=n.m_im>0?dynmargin:stamargin;
     808                btSoftBody::RContact    c;
     809                if(     (!n.m_battach)&&
     810                        psb->checkContact(prb,n.m_x,m,c.m_cti))
    811811                        {
    812                                 const btScalar  ima=n.m_im;
    813                                 const btScalar  imb=prb->getInvMass();
    814                                 const btScalar  ms=ima+imb;
    815                                 if(ms>0)
     812                        const btScalar  ima=n.m_im;
     813                        const btScalar  imb=prb->getInvMass();
     814                        const btScalar  ms=ima+imb;
     815                        if(ms>0)
    816816                                {
    817                                         const btTransform&      wtr=prb->getInterpolationWorldTransform();
    818                                         const btMatrix3x3&      iwi=prb->getInvInertiaTensorWorld();
    819                                         const btVector3         ra=n.m_x-wtr.getOrigin();
    820                                         const btVector3         va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
    821                                         const btVector3         vb=n.m_x-n.m_q;
    822                                         const btVector3         vr=vb-va;
    823                                         const btScalar          dn=dot(vr,c.m_cti.m_normal);
    824                                         const btVector3         fv=vr-c.m_cti.m_normal*dn;
    825                                         const btScalar          fc=psb->m_cfg.kDF*prb->getFriction();
    826                                         c.m_node        =       &n;
    827                                         c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
    828                                         c.m_c1          =       ra;
    829                                         c.m_c2          =       ima*psb->m_sst.sdt;
    830                                         c.m_c3          =       fv.length2()<(btFabs(dn)*fc)?0:1-fc;
    831                                         c.m_c4          =       prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
    832                                         psb->m_rcontacts.push_back(c);
    833                                         prb->activate();
     817                                const btTransform&      wtr=prb->getInterpolationWorldTransform();
     818                                const btMatrix3x3&      iwi=prb->getInvInertiaTensorWorld();
     819                                const btVector3         ra=n.m_x-wtr.getOrigin();
     820                                const btVector3         va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
     821                                const btVector3         vb=n.m_x-n.m_q;
     822                                const btVector3         vr=vb-va;
     823                                const btScalar          dn=dot(vr,c.m_cti.m_normal);
     824                                const btVector3         fv=vr-c.m_cti.m_normal*dn;
     825                                const btScalar          fc=psb->m_cfg.kDF*prb->getFriction();
     826                                c.m_node        =       &n;
     827                                c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
     828                                c.m_c1          =       ra;
     829                                c.m_c2          =       ima*psb->m_sst.sdt;
     830                                c.m_c3          =       fv.length2()<(btFabs(dn)*fc)?0:1-fc;
     831                                c.m_c4          =       prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
     832                                psb->m_rcontacts.push_back(c);
     833                                prb->activate();
    834834                                }
    835835                        }
    836836                }
    837                 btSoftBody*             psb;
    838                 btRigidBody*    prb;
    839                 btScalar                dynmargin;
    840                 btScalar                stamargin;
     837        btSoftBody*             psb;
     838        btRigidBody*    prb;
     839        btScalar                dynmargin;
     840        btScalar                stamargin;
    841841        };
    842842        //
    … …  
    845845        struct  CollideVF_SS : btDbvt::ICollide
    846846        {
    847                 void            Process(const btDbvtNode* lnode,
    848                         const btDbvtNode* lface)
    849                 {
    850                         btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
    851                         btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
    852                         btVector3                       o=node->m_x;
    853                         btVector3                       p;
    854                         btScalar                        d=SIMD_INFINITY;
    855                         ProjectOrigin(  face->m_n[0]->m_x-o,
    856                                 face->m_n[1]->m_x-o,
    857                                 face->m_n[2]->m_x-o,
    858                                 p,d);
    859                         const btScalar  m=mrg+(o-node->m_q).length()*2;
    860                         if(d<(m*m))
     847        void            Process(const btDbvtNode* lnode,
     848                                                const btDbvtNode* lface)
     849                {
     850                btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
     851                btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
     852                btVector3                       o=node->m_x;
     853                btVector3                       p;
     854                btScalar                        d=SIMD_INFINITY;
     855                ProjectOrigin(  face->m_n[0]->m_x-o,
     856                                                face->m_n[1]->m_x-o,
     857                                                face->m_n[2]->m_x-o,
     858                                                p,d);
     859                const btScalar  m=mrg+(o-node->m_q).length()*2;
     860                if(d<(m*m))
    861861                        {
    862                                 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
    863                                 const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
    864                                 const btScalar                  ma=node->m_im;
    865                                 btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
    866                                 if(     (n[0]->m_im<=0)||
    867                                         (n[1]->m_im<=0)||
    868                                         (n[2]->m_im<=0))
     862                        const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
     863                        const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
     864                        const btScalar                  ma=node->m_im;
     865                        btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
     866                        if(     (n[0]->m_im<=0)||
     867                                (n[1]->m_im<=0)||
     868                                (n[2]->m_im<=0))
    869869                                {
    870                                         mb=0;
     870                                mb=0;
    871871                                }
    872                                 const btScalar  ms=ma+mb;
    873                                 if(ms>0)
     872                        const btScalar  ms=ma+mb;
     873                        if(ms>0)
    874874                                {
    875                                         btSoftBody::SContact    c;
    876                                         c.m_normal              =       p/-btSqrt(d);
    877                                         c.m_margin              =       m;
    878                                         c.m_node                =       node;
    879                                         c.m_face                =       face;
    880                                         c.m_weights             =       w;
    881                                         c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
    882                                         c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
    883                                         c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
    884                                         psb[0]->m_scontacts.push_back(c);
     875                                btSoftBody::SContact    c;
     876                                c.m_normal              =       p/-btSqrt(d);
     877                                c.m_margin              =       m;
     878                                c.m_node                =       node;
     879                                c.m_face                =       face;
     880                                c.m_weights             =       w;
     881                                c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
     882                                c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
     883                                c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
     884                                psb[0]->m_scontacts.push_back(c);
    885885                                }
    886886                        }       
    887887                }
    888                 btSoftBody*             psb[2];
    889                 btScalar                mrg;
     888        btSoftBody*             psb[2];
     889        btScalar                mrg;
    890890        };
    891891};

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp

    r1963 r1972  
    3030        mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
    3131        m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;
    32 
     32       
    3333        mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
    3434        m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
    35 
     35       
    3636        mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
    3737        m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
    … …  
    4141        mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
    4242        m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
    43 
     43       
    4444        mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
    4545        m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
    … …  
    4848
    4949        //replace pool by a new one, with potential larger size
    50 
     50       
    5151        if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool)
    5252        {
    5353                int curElemSize = m_collisionAlgorithmPool->getElementSize();
    5454                ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
    55 
    56 
     55               
     56               
    5757                int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm);
    5858                int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm);
    … …  
    9393#endif
    9494}
    95 
     95       
    9696///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
    9797btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h

    r1963 r1972  
    3333        btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc;
    3434        btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc;
    35 
     35       
    3636public:
    3737

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp

    r1963 r1972  
    3636btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
    3737{
    38 
     38       
    3939        //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
    4040
    4141        /*if (m_ownManifold)
    4242        {
    43         if (m_manifoldPtr)
    44         m_dispatcher->releaseManifold(m_manifoldPtr);
     43                if (m_manifoldPtr)
     44                        m_dispatcher->releaseManifold(m_manifoldPtr);
    4545        }
    4646        */
    … …  
    5959        btSoftBody* softBody =  m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0;
    6060        btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1;
    61 
     61       
    6262        softBody->defaultCollisionHandler(rigidCollisionObject);
    6363

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.h

    r1963 r1972  
    2929class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
    3030{
    31         //      bool    m_ownManifold;
    32         //      btPersistentManifold*   m_manifoldPtr;
     31//      bool    m_ownManifold;
     32//      btPersistentManifold*   m_manifoldPtr;
    3333
    3434        btSoftBody*                             m_softBody;
    … …  
    3737        ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean
    3838        bool    m_isSwapped;
    39 
     39       
    4040public:
    4141
    … …  
    5353        }
    5454
    55 
     55       
    5656        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
    5757        {

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.cpp

    r1963 r1972  
    2929:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
    3030{
    31         m_drawFlags                     =       fDrawFlags::Std;
    32         m_drawNodeTree          =       true;
    33         m_drawFaceTree          =       false;
    34         m_drawClusterTree       =       false;
    35         m_sbi.m_broadphase = pairCache;
    36         m_sbi.m_dispatcher = dispatcher;
    37         m_sbi.m_sparsesdf.Initialize();
    38         m_sbi.m_sparsesdf.Reset();
     31m_drawFlags                     =       fDrawFlags::Std;
     32m_drawNodeTree          =       true;
     33m_drawFaceTree          =       false;
     34m_drawClusterTree       =       false;
     35m_sbi.m_broadphase = pairCache;
     36m_sbi.m_dispatcher = dispatcher;
     37m_sbi.m_sparsesdf.Initialize();
     38m_sbi.m_sparsesdf.Reset();
    3939
    4040}
    41 
     41               
    4242btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
    4343{
    … …  
    5656        }
    5757}
    58 
     58               
    5959void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
    6060{
    … …  
    7272{
    7373        BT_PROFILE("updateSoftBodies");
    74 
     74       
    7575        for ( int i=0;i<m_softBodies.size();i++)
    7676        {
    … …  
    8383{
    8484        BT_PROFILE("solveSoftConstraints");
    85 
     85       
    8686        if(m_softBodies.size())
    87         {
     87                {
    8888                btSoftBody::solveClusters(m_softBodies);
    89         }
    90 
     89                }
     90       
    9191        for(int i=0;i<m_softBodies.size();++i)
    9292        {
    … …  
    101101
    102102        btCollisionWorld::addCollisionObject(body,
    103                 btBroadphaseProxy::DefaultFilter,
    104                 btBroadphaseProxy::AllFilter);
     103                                        btBroadphaseProxy::DefaultFilter,
     104                                        btBroadphaseProxy::AllFilter);
    105105
    106106}

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.h

    r1963 r1972  
    2424class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
    2525{
    26 
     26       
    2727        btSoftBodyArray m_softBodies;
    2828        int                             m_drawFlags;
    … …  
    3333
    3434protected:
    35 
     35       
    3636        virtual void    predictUnconstraintMotion(btScalar timeStep);
    37 
     37       
    3838        virtual void    internalSingleStepSimulation( btScalar timeStep);
    3939
    … …  
    4444
    4545public:
    46 
     46       
    4747        btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
    4848
    4949        virtual ~btSoftRigidDynamicsWorld();
    50 
     50       
    5151        virtual void    debugDrawWorld();
    52 
     52                       
    5353        void    addSoftBody(btSoftBody* body);
    5454
    5555        void    removeSoftBody(btSoftBody* body);
    56 
     56       
    5757        int             getDrawFlags() const { return(m_drawFlags); }
    5858        void    setDrawFlags(int f)     { m_drawFlags=f; }
    … …  
    6767        }
    6868
    69 
     69                       
    7070        btSoftBodyArray& getSoftBodyArray()
    7171        {
    … …  
    7777                return m_softBodies;
    7878        }
    79 
     79               
    8080};
    8181

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.h

    r1963 r1972  
    3434        btSoftBody*     m_softBody1;
    3535
    36 
     36       
    3737public:
    3838        btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)

  • code/branches/physics/src/bullet/BulletSoftBody/btSparseSDF.h

    r1963 r1972  
    2424template <const int DWORDLEN>
    2525unsigned int HsiehHash(const void* pdata)
    26 {
     26        {
    2727        const unsigned short*   data=(const unsigned short*)pdata;
    2828        unsigned                                hash=DWORDLEN<<2,tmp;
    2929        for(int i=0;i<DWORDLEN;++i)
    30         {
     30                {
    3131                hash    +=      data[0];
    3232                tmp             =       (data[1]<<11)^hash;
    … …  
    3434                data    +=      2;
    3535                hash    +=      hash>>11;
    36         }
     36                }
    3737        hash^=hash<<3;hash+=hash>>5;
    3838        hash^=hash<<4;hash+=hash>>17;
    3939        hash^=hash<<25;hash+=hash>>6;
    4040        return(hash);
    41 }
     41        }
    4242
    4343template <const int CELLSIZE>
    4444struct  btSparseSdf
    45 {
     45        {
    4646        //
    4747        // Inner types
    4848        //
    4949        struct IntFrac
    50         {
     50                {
    5151                int                                     b;
    5252                int                                     i;
    5353                btScalar                        f;
    54         };
     54                };
    5555        struct  Cell
    56         {
     56                {
    5757                btScalar                        d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
    5858                int                                     c[3];
    … …  
    6161                btCollisionShape*       pclient;
    6262                Cell*                           next;
    63         };
     63                };
    6464        //
    6565        // Fields
    6666        //
    67 
     67       
    6868        btAlignedObjectArray<Cell*>             cells; 
    6969        btScalar                                                voxelsz;
    … …  
    7272        int                                                             nprobes;
    7373        int                                                             nqueries;       
    74 
     74       
    7575        //
    7676        // Methods
    7777        //
    78 
     78       
    7979        //
    8080        void                                    Initialize(int hashsize=2383)
    81         {
     81                {
    8282                cells.resize(hashsize,0);
    8383                Reset();               
    84         }
     84                }
    8585        //
    8686        void                                    Reset()
    87         {
     87                {
    8888                for(int i=0,ni=cells.size();i<ni;++i)
    89                 {
     89                        {
    9090                        Cell*   pc=cells[i];
    9191                        cells[i]=0;
    9292                        while(pc)
    93                         {
     93                                {
    9494                                Cell*   pn=pc->next;
    9595                                delete pc;
    9696                                pc=pn;
    97                         }
    98                 }
     97                                }
     98                        }
    9999                voxelsz         =0.25;
    100100                puid            =0;
    … …  
    102102                nprobes         =1;
    103103                nqueries        =1;
    104         }
     104                }
    105105        //
    106106        void                                    GarbageCollect(int lifetime=256)
    107         {
     107                {
    108108                const int life=puid-lifetime;
    109109                for(int i=0;i<cells.size();++i)
    110                 {
     110                        {
    111111                        Cell*&  root=cells[i];
    112112                        Cell*   pp=0;
    113113                        Cell*   pc=root;
    114114                        while(pc)
    115                         {
     115                                {
    116116                                Cell*   pn=pc->next;
    117117                                if(pc->puid<life)
    118                                 {
     118                                        {
    119119                                        if(pp) pp->next=pn; else root=pn;
    120120                                        delete pc;pc=pp;--ncells;
     121                                        }
     122                                pp=pc;pc=pn;
    121123                                }
    122                                 pp=pc;pc=pn;
    123                         }
    124                 }
     124                        }
    125125                //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
    126126                nqueries=1;
    127127                nprobes=1;
    128128                ++puid; /* TODO: Reset puid's when int range limit is reached   */
    129                 /* else setup a priority list...                                                */
    130         }
     129                                /* else setup a priority list...                                                */
     130                }
    131131        //
    132132        int                                             RemoveReferences(btCollisionShape* pcs)
    133         {
     133                {
    134134                int     refcount=0;
    135135                for(int i=0;i<cells.size();++i)
    136                 {
     136                        {
    137137                        Cell*&  root=cells[i];
    138138                        Cell*   pp=0;
    139139                        Cell*   pc=root;
    140140                        while(pc)
    141                         {
     141                                {
    142142                                Cell*   pn=pc->next;
    143143                                if(pc->pclient==pcs)
    144                                 {
     144                                        {
    145145                                        if(pp) pp->next=pn; else root=pn;
    146146                                        delete pc;pc=pp;++refcount;
     147                                        }
     148                                pp=pc;pc=pn;
    147149                                }
    148                                 pp=pc;pc=pn;
    149                         }
    150                 }
     150                        }
    151151                return(refcount);
    152         }
     152                }
    153153        //
    154154        btScalar                                Evaluate(       const btVector3& x,
    155                 btCollisionShape* shape,
    156                 btVector3& normal,
    157                 btScalar margin)
    158         {
     155                                                                                btCollisionShape* shape,
     156                                                                                btVector3& normal,
     157                                                                                btScalar margin)
     158                {
    159159                /* Lookup cell                  */
    160160                const btVector3 scx=x/voxelsz;
    … …  
    167167                ++nqueries;
    168168                while(c)
    169                 {
     169                        {
    170170                        ++nprobes;
    171171                        if(     (c->hash==h)    &&
    … …  
    174174                                (c->c[2]==iz.b) &&
    175175                                (c->pclient==shape))
    176                         { break; }
    177                         else
    178                         { c=c->next; }
    179                 }
     176                                { break; }
     177                                else
     178                                { c=c->next; }
     179                        }
    180180                if(!c)
    181                 {
     181                        {
    182182                        ++nprobes;             
    183183                        ++ncells;
    … …  
    188188                        c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
    189189                        BuildCell(*c);
    190                 }
     190                        }
    191191                c->puid=puid;
    192192                /* Extract infos                */
    193193                const int               o[]={   ix.i,iy.i,iz.i};
    194194                const btScalar  d[]={   c->d[o[0]+0][o[1]+0][o[2]+0],
    195                         c->d[o[0]+1][o[1]+0][o[2]+0],
    196                         c->d[o[0]+1][o[1]+1][o[2]+0],
    197                         c->d[o[0]+0][o[1]+1][o[2]+0],
    198                         c->d[o[0]+0][o[1]+0][o[2]+1],
    199                         c->d[o[0]+1][o[1]+0][o[2]+1],
    200                         c->d[o[0]+1][o[1]+1][o[2]+1],
    201                         c->d[o[0]+0][o[1]+1][o[2]+1]};
     195                                                                c->d[o[0]+1][o[1]+0][o[2]+0],
     196                                                                c->d[o[0]+1][o[1]+1][o[2]+0],
     197                                                                c->d[o[0]+0][o[1]+1][o[2]+0],
     198                                                                c->d[o[0]+0][o[1]+0][o[2]+1],
     199                                                                c->d[o[0]+1][o[1]+0][o[2]+1],
     200                                                                c->d[o[0]+1][o[1]+1][o[2]+1],
     201                                                                c->d[o[0]+0][o[1]+1][o[2]+1]};
    202202                /* Normal       */
    203 #if 1
     203                #if 1
    204204                const btScalar  gx[]={  d[1]-d[0],d[2]-d[3],
    205                         d[5]-d[4],d[6]-d[7]};
     205                                                                d[5]-d[4],d[6]-d[7]};
    206206                const btScalar  gy[]={  d[3]-d[0],d[2]-d[1],
    207                         d[7]-d[4],d[6]-d[5]};
     207                                                                d[7]-d[4],d[6]-d[5]};
    208208                const btScalar  gz[]={  d[4]-d[0],d[5]-d[1],
    209                         d[7]-d[3],d[6]-d[2]};
     209                                                                d[7]-d[3],d[6]-d[2]};
    210210                normal.setX(Lerp(       Lerp(gx[0],gx[1],iy.f),
    211                         Lerp(gx[2],gx[3],iy.f),iz.f));
     211                                                        Lerp(gx[2],gx[3],iy.f),iz.f));
    212212                normal.setY(Lerp(       Lerp(gy[0],gy[1],ix.f),
    213                         Lerp(gy[2],gy[3],ix.f),iz.f));
     213                                                        Lerp(gy[2],gy[3],ix.f),iz.f));
    214214                normal.setZ(Lerp(       Lerp(gz[0],gz[1],ix.f),
    215                         Lerp(gz[2],gz[3],ix.f),iy.f));
     215                                                        Lerp(gz[2],gz[3],ix.f),iy.f));
    216216                normal          =       normal.normalized();
    217 #else
     217                #else
    218218                normal          =       btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
    219 #endif
     219                #endif
    220220                /* Distance     */
    221221                const btScalar  d0=Lerp(Lerp(d[0],d[1],ix.f),
    222                         Lerp(d[3],d[2],ix.f),iy.f);
     222                                                                Lerp(d[3],d[2],ix.f),iy.f);
    223223                const btScalar  d1=Lerp(Lerp(d[4],d[5],ix.f),
    224                         Lerp(d[7],d[6],ix.f),iy.f);
     224                                                                Lerp(d[7],d[6],ix.f),iy.f);
    225225                return(Lerp(d0,d1,iz.f)-margin);
    226         }
     226                }
    227227        //
    228228        void                                    BuildCell(Cell& c)
    229         {
     229                {
    230230                const btVector3 org=btVector3(  (btScalar)c.c[0],
    231                         (btScalar)c.c[1],
    232                         (btScalar)c.c[2])       *
    233                         CELLSIZE*voxelsz;
     231                                                                                (btScalar)c.c[1],
     232                                                                                (btScalar)c.c[2])       *
     233                                                                                CELLSIZE*voxelsz;
    234234                for(int k=0;k<=CELLSIZE;++k)
    235                 {
     235                        {
    236236                        const btScalar  z=voxelsz*k+org.z();
    237237                        for(int j=0;j<=CELLSIZE;++j)
    238                         {
     238                                {
    239239                                const btScalar  y=voxelsz*j+org.y();
    240240                                for(int i=0;i<=CELLSIZE;++i)
    241                                 {
     241                                        {
    242242                                        const btScalar  x=voxelsz*i+org.x();
    243243                                        c.d[i][j][k]=DistanceToShape(   btVector3(x,y,z),
    244                                                 c.pclient);
     244                                                                                                        c.pclient);
     245                                        }
    245246                                }
    246247                        }
    247248                }
    248         }
    249249        //
    250250        static inline btScalar  DistanceToShape(const btVector3& x,
    251                 btCollisionShape* shape)
    252         {
     251                                                                                        btCollisionShape* shape)
     252                {
    253253                btTransform     unit;
    254254                unit.setIdentity();
    255255                if(shape->isConvex())
    256                 {
     256                        {
    257257                        btGjkEpaSolver2::sResults       res;
    258258                        btConvexShape*                          csh=static_cast<btConvexShape*>(shape);
    259259                        return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
    260                 }
     260                        }
    261261                return(0);
    262         }
     262                }
    263263        //
    264264        static inline IntFrac   Decompose(btScalar x)
    265         {
     265                {
    266266                /* That one need a lot of improvements...       */
    267267                /* Remove test, faster floor...                         */
    … …  
    273273                r.i=(int)k;r.f=k-r.i;r.b-=o;
    274274                return(r);
    275         }
     275                }
    276276        //
    277277        static inline btScalar  Lerp(btScalar a,btScalar b,btScalar t)
    278         {
     278                {
    279279                return(a+(b-a)*t);
    280         }
    281 
    282 
     280                }
     281
     282       
    283283
    284284        //
    285285        static inline unsigned int      Hash(int x,int y,int z,btCollisionShape* shape)
    286         {
     286                {
    287287                struct btS
    288288                {
    … …  
    292292
    293293                btS myset;
    294 
     294               
    295295                myset.x=x;myset.y=y;myset.z=z;myset.p=shape;
    296296                const void* ptr = &myset;
    297297
    298298                unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
    299 
     299               
    300300
    301301                return result;
    302         }
     302                }
    303303};
    304 
     304       
    305305
    306306#endif

  • code/branches/physics/src/bullet/CMakeLists.txt

    r1963 r1972  
    1 if (CMAKE_SIZEOF_VOID_P MATCHES "8")
    2   SUBDIRS( BulletSoftBody BulletCollision BulletDynamics LinearMath )
    3 else (CMAKE_SIZEOF_VOID_P MATCHES "8")
    4   SUBDIRS( BulletMultiThreaded BulletSoftBody BulletCollision BulletDynamics LinearMath )
    5 endif (CMAKE_SIZEOF_VOID_P MATCHES "8")
     1INCLUDE_DIRECTORIES(.)
     2
     3ADD_SUBDIRECTORY(BulletSoftBody)
     4ADD_SUBDIRECTORY(BulletCollision)
     5ADD_SUBDIRECTORY(BulletDynamics)
     6ADD_SUBDIRECTORY(BulletLinearMath)

  • code/branches/physics/src/bullet/ChangeLog

    r1963 r1972  
    55See http://code.google.com/p/bullet/source/list for more complete log in Subversion
    66
    7 2008 October 10
    8         - Moved aabb to btBroadphaseProxy, improves rayTest dramatically. Further raytest improvements using the broadphase acceleration structures are planned
    97
    1082008 October 3

  • code/branches/physics/src/bullet/LinearMath/btScalar.h

    r1963 r1972  
    2525#include <float.h>
    2626
    27 #define BT_BULLET_VERSION 273
     27#define BT_BULLET_VERSION 272
    2828
    2929inline int      btGetVersion()
Note: See TracChangeset for help on using the changeset viewer.