Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Updated to Bullet 2.73 (first part).

Location:
code/branches/physics/src/bullet
Files:
8 added
5 deleted
135 edited

Legend:

Unmodified
Added
Removed

  • code/branches/physics/src/bullet/Bullet-C-Api.h

    r2192 r2430  
    3939#endif
    4040
    41 /*      Particular physics SDK */
     41/**     Particular physics SDK (C-API) */
    4242        PL_DECLARE_HANDLE(plPhysicsSdkHandle);
    4343
    44 /*      Dynamics world, belonging to some physics SDK */
     44/**     Dynamics world, belonging to some physics SDK (C-API)*/
    4545        PL_DECLARE_HANDLE(plDynamicsWorldHandle);
    4646
    47 /* Rigid Body that can be part of a Dynamics World */   
     47/** Rigid Body that can be part of a Dynamics World (C-API)*/   
    4848        PL_DECLARE_HANDLE(plRigidBodyHandle);
    4949
    50 /*      Collision Shape/Geometry, property of a Rigid Body */
     50/**     Collision Shape/Geometry, property of a Rigid Body (C-API)*/
    5151        PL_DECLARE_HANDLE(plCollisionShapeHandle);
    5252
    53 /* Constraint for Rigid Bodies */
     53/** Constraint for Rigid Bodies (C-API)*/
    5454        PL_DECLARE_HANDLE(plConstraintHandle);
    5555
    56 /* Triangle Mesh interface */
     56/** Triangle Mesh interface (C-API)*/
    5757        PL_DECLARE_HANDLE(plMeshInterfaceHandle);
    5858
    59 /* Broadphase Scene/Proxy Handles */
     59/** Broadphase Scene/Proxy Handles (C-API)*/
    6060        PL_DECLARE_HANDLE(plCollisionBroadphaseHandle);
    6161        PL_DECLARE_HANDLE(plBroadphaseProxyHandle);
    6262        PL_DECLARE_HANDLE(plCollisionWorldHandle);
    6363
    64 /*
     64/**
    6565        Create and Delete a Physics SDK
    6666*/
    … …  
    6969        extern  void            plDeletePhysicsSdk(plPhysicsSdkHandle   physicsSdk);
    7070
    71 /* Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
     71/** Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
    7272
    7373        typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2);

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

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

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

    r2192 r2430  
    2020#define AXIS_SWEEP_3_H
    2121
    22 #include "LinearMath/btPoint3.h"
    2322#include "LinearMath/btVector3.h"
    2423#include "btOverlappingPairCache.h"
    … …  
    2625#include "btBroadphaseProxy.h"
    2726#include "btOverlappingPairCallback.h"
     27#include "btDbvtBroadphase.h"
    2828
    2929//#define DEBUG_BROADPHASE 1
    … …  
    6262                BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];            // 6 * 2 = 12
    6363//              BP_FP_INT_TYPE m_uniqueId;
    64                 BP_FP_INT_TYPE m_pad;
    65                
     64                btBroadphaseProxy*      m_dbvtProxy;//for faster raycast
    6665                //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
    6766       
    … …  
    7271       
    7372protected:
    74         btPoint3 m_worldAabbMin;                                                // overall system bounds
    75         btPoint3 m_worldAabbMax;                                                // overall system bounds
     73        btVector3 m_worldAabbMin;                                               // overall system bounds
     74        btVector3 m_worldAabbMax;                                               // overall system bounds
    7675
    7776        btVector3 m_quantize;                                           // scaling factor for quantization
    … …  
    9594        int     m_invalidPair;
    9695
     96        ///additional dynamic aabb structure, used to accelerate ray cast queries.
     97        ///can be disabled using a optional argument in the constructor
     98        btDbvtBroadphase*       m_raycastAccelerator;
     99        btOverlappingPairCache* m_nullPairCache;
     100
     101
    97102        // allocation/deallocation
    98103        BP_FP_INT_TYPE allocHandle();
    … …  
    109114        //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
    110115
    111         void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
     116       
    112117
    113118        void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
    … …  
    118123public:
    119124
    120         btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0);
     125        btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);
    121126
    122127        virtual ~btAxisSweep3Internal();
    … …  
    129134        virtual void    calculateOverlappingPairs(btDispatcher* dispatcher);
    130135       
    131         BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
     136        BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
    132137        void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
    133         void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
     138        void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
    134139        SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
     140
     141        void resetPool();
    135142
    136143        void    processAllOverlappingPairs(btOverlapCallback* callback);
    … …  
    140147        virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
    141148        virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
     149        virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
     150       
     151        virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
     152       
     153        void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
     154        ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
     155        void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
    142156       
    143157        bool    testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
    … …  
    218232               
    219233                Handle* handle = getHandle(handleId);
    220                                
     234               
     235                if (m_raycastAccelerator)
     236                {
     237                        btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
     238                        handle->m_dbvtProxy = rayProxy;
     239                }
    221240                return handle;
    222241}
    … …  
    228247{
    229248        Handle* handle = static_cast<Handle*>(proxy);
     249        if (m_raycastAccelerator)
     250                m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
    230251        removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
    231252}
    … …  
    235256{
    236257        Handle* handle = static_cast<Handle*>(proxy);
     258        handle->m_aabbMin = aabbMin;
     259        handle->m_aabbMax = aabbMax;
    237260        updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
    238 
    239 }
    240 
    241 
    242 
    243 
    244 
    245 template <typename BP_FP_INT_TYPE>
    246 btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache )
     261        if (m_raycastAccelerator)
     262                m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
     263
     264}
     265
     266template <typename BP_FP_INT_TYPE>
     267void    btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
     268{
     269        if (m_raycastAccelerator)
     270        {
     271                m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
     272        } else
     273        {
     274                //choose axis?
     275                BP_FP_INT_TYPE axis = 0;
     276                //for each proxy
     277                for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
     278                {
     279                        if (m_pEdges[axis][i].IsMax())
     280                        {
     281                                rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
     282                        }
     283                }
     284        }
     285}
     286
     287
     288
     289template <typename BP_FP_INT_TYPE>
     290void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
     291{
     292        Handle* pHandle = static_cast<Handle*>(proxy);
     293        aabbMin = pHandle->m_aabbMin;
     294        aabbMax = pHandle->m_aabbMax;
     295}
     296
     297
     298template <typename BP_FP_INT_TYPE>
     299void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
     300{
     301        Handle* pHandle = static_cast<Handle*>(proxy);
     302
     303        unsigned short vecInMin[3];
     304        unsigned short vecInMax[3];
     305
     306        vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
     307        vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
     308        vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
     309        vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
     310        vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
     311        vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
     312       
     313        aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
     314        aabbMin += m_worldAabbMin;
     315       
     316        aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
     317        aabbMax += m_worldAabbMin;
     318}
     319
     320
     321
     322
     323template <typename BP_FP_INT_TYPE>
     324btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
    247325:m_bpHandleMask(handleMask),
    248326m_handleSentinel(handleSentinel),
    … …  
    250328m_userPairCallback(0),
    251329m_ownsPairCache(false),
    252 m_invalidPair(0)
     330m_invalidPair(0),
     331m_raycastAccelerator(0)
    253332{
    254333        BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
    … …  
    259338                m_pairCache = new(ptr) btHashedOverlappingPairCache();
    260339                m_ownsPairCache = true;
     340        }
     341
     342        if (!disableRaycastAccelerator)
     343        {
     344                m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache();
     345                m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache);
     346                m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
    261347        }
    262348
    … …  
    321407btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
    322408{
    323        
     409        if (m_raycastAccelerator)
     410        {
     411                m_nullPairCache->~btOverlappingPairCache();
     412                btAlignedFree(m_nullPairCache);
     413                m_raycastAccelerator->~btDbvtBroadphase();
     414                btAlignedFree (m_raycastAccelerator);
     415        }
     416
    324417        for (int i = 2; i >= 0; i--)
    325418        {
    … …  
    336429
    337430template <typename BP_FP_INT_TYPE>
    338 void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
     431void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
    339432{
    340433#ifdef OLD_CLAMPING_METHOD
    341434        ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax]
    342435        ///see http://code.google.com/p/bullet/issues/detail?id=87
    343         btPoint3 clampedPoint(point);
     436        btVector3 clampedPoint(point);
    344437        clampedPoint.setMax(m_worldAabbMin);
    345438        clampedPoint.setMin(m_worldAabbMax);
    … …  
    382475
    383476template <typename BP_FP_INT_TYPE>
    384 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
     477BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
    385478{
    386479        // quantize the bounds
    … …  
    445538        //explicitly remove the pairs containing the proxy
    446539        //we could do it also in the sortMinUp (passing true)
    447         //todo: compare performance
     540        ///@todo: compare performance
    448541        if (!m_pairCache->hasDeferredRemoval())
    449542        {
    … …  
    493586       
    494587}
     588
     589template <typename BP_FP_INT_TYPE>
     590void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool()
     591{
     592        if (m_numHandles == 0)
     593        {
     594                m_firstFreeHandle = 1;
     595                {
     596                        for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
     597                                m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
     598                        m_pHandles[m_maxHandles - 1].SetNextFree(0);
     599                }
     600        }
     601}       
     602
    495603
    496604extern int gOverlappingPairs;
    … …  
    621729
    622730template <typename BP_FP_INT_TYPE>
    623 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
     731void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
    624732{
    625733//      assert(bounds.IsFinite());
    … …  
    9001008public:
    9011009
    902         btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
     1010        btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
    9031011
    9041012};
    … …  
    9111019public:
    9121020
    913         bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
     1021        bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
    9141022
    9151023};

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    r2192 r2430  
    1 ADD_LIBRARY(LibBulletCollision
    2                                 BroadphaseCollision/btAxisSweep3.cpp
    3                                 BroadphaseCollision/btAxisSweep3.h
    4                                 BroadphaseCollision/btBroadphaseProxy.cpp
    5                                 BroadphaseCollision/btBroadphaseProxy.h
    6                                 BroadphaseCollision/btCollisionAlgorithm.cpp
    7                                 BroadphaseCollision/btCollisionAlgorithm.h
    8                                 BroadphaseCollision/btDispatcher.cpp
    9                                 BroadphaseCollision/btDispatcher.h
    10                                 BroadphaseCollision/btDbvtBroadphase.cpp
    11                                 BroadphaseCollision/btDbvtBroadphase.h
    12                                 BroadphaseCollision/btDbvt.cpp
    13                                 BroadphaseCollision/btDbvt.h
    14                                 BroadphaseCollision/btMultiSapBroadphase.cpp
    15                                 BroadphaseCollision/btMultiSapBroadphase.h
    16                                 BroadphaseCollision/btOverlappingPairCache.cpp
    17                                 BroadphaseCollision/btOverlappingPairCache.h
    18                                 BroadphaseCollision/btOverlappingPairCallback.h
    19                                 BroadphaseCollision/btQuantizedBvh.cpp
    20                                 BroadphaseCollision/btQuantizedBvh.h
    21                                 BroadphaseCollision/btSimpleBroadphase.cpp
    22                                 BroadphaseCollision/btSimpleBroadphase.h
    23                                 CollisionDispatch/btCollisionDispatcher.cpp
    24                                 CollisionDispatch/btCollisionDispatcher.h
    25                                 CollisionDispatch/btCollisionObject.cpp
    26                                 CollisionDispatch/btCollisionObject.h
    27                                 CollisionDispatch/btCollisionWorld.cpp
    28                                 CollisionDispatch/btCollisionWorld.h
    29                                 CollisionDispatch/btCompoundCollisionAlgorithm.cpp
    30                                 CollisionDispatch/btCompoundCollisionAlgorithm.h
    31                                 CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
    32                                 CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
    33                                 CollisionDispatch/btDefaultCollisionConfiguration.cpp
    34                                 CollisionDispatch/btDefaultCollisionConfiguration.h
    35                                 CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
    36                                 CollisionDispatch/btSphereSphereCollisionAlgorithm.h
    37                                 CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
    38                                 CollisionDispatch/btBoxBoxCollisionAlgorithm.h
    39                                 CollisionDispatch/btBoxBoxDetector.cpp
    40                                 CollisionDispatch/btBoxBoxDetector.h
    41                                 CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
    42                                 CollisionDispatch/btSphereBoxCollisionAlgorithm.h
    43                                 CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
    44                                 CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
    45                                 CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
    46                                 CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
    47                                 CollisionDispatch/btConvexConvexAlgorithm.cpp
    48                                 CollisionDispatch/btConvexConvexAlgorithm.h
    49                                 CollisionDispatch/btEmptyCollisionAlgorithm.cpp
    50                                 CollisionDispatch/btEmptyCollisionAlgorithm.h
    51                                 CollisionDispatch/btManifoldResult.cpp
    52                                 CollisionDispatch/btManifoldResult.h
    53                                 CollisionDispatch/btSimulationIslandManager.cpp
    54                                 CollisionDispatch/btSimulationIslandManager.h
    55                                 CollisionDispatch/btUnionFind.cpp
    56                                 CollisionDispatch/btUnionFind.h
    57                                 CollisionDispatch/SphereTriangleDetector.cpp
    58                                 CollisionDispatch/SphereTriangleDetector.h
    59                                 CollisionShapes/btBoxShape.cpp
    60                                 CollisionShapes/btBoxShape.h
    61                                 CollisionShapes/btBvhTriangleMeshShape.cpp
    62                                 CollisionShapes/btBvhTriangleMeshShape.h
    63                                 CollisionShapes/btCapsuleShape.cpp
    64                                 CollisionShapes/btCapsuleShape.h
    65                                 CollisionShapes/btCollisionShape.cpp
    66                                 CollisionShapes/btCollisionShape.h
    67                                 CollisionShapes/btCompoundShape.cpp
    68                                 CollisionShapes/btCompoundShape.h
    69                                 CollisionShapes/btConcaveShape.cpp
    70                                 CollisionShapes/btConcaveShape.h
    71                                 CollisionShapes/btConeShape.cpp
    72                                 CollisionShapes/btConeShape.h
    73                                 CollisionShapes/btConvexHullShape.cpp
    74                                 CollisionShapes/btConvexHullShape.h
    75                                 CollisionShapes/btConvexPointCloudShape.cpp
    76                                 CollisionShapes/btConvexPointCloudShape.h
    77                                 CollisionShapes/btConvexShape.cpp
    78                                 CollisionShapes/btConvexShape.h
    79                                 CollisionShapes/btConvexInternalShape.cpp
    80                                 CollisionShapes/btConvexInternalShape.h
    81                                 CollisionShapes/btConvexTriangleMeshShape.cpp
    82                                 CollisionShapes/btConvexTriangleMeshShape.h
    83                                 CollisionShapes/btCylinderShape.cpp
    84                                 CollisionShapes/btCylinderShape.h
    85                                 CollisionShapes/btEmptyShape.cpp
    86                                 CollisionShapes/btEmptyShape.h
    87                                 CollisionShapes/btHeightfieldTerrainShape.cpp
    88                                 CollisionShapes/btHeightfieldTerrainShape.h
    89                                 CollisionShapes/btMinkowskiSumShape.cpp
    90                                 CollisionShapes/btMinkowskiSumShape.h
    91                                 CollisionShapes/btMaterial.h
    92                                 CollisionShapes/btMultimaterialTriangleMeshShape.cpp
    93                                 CollisionShapes/btMultimaterialTriangleMeshShape.h
    94                                 CollisionShapes/btMultiSphereShape.cpp
    95                                 CollisionShapes/btMultiSphereShape.h
    96                                 CollisionShapes/btOptimizedBvh.cpp
    97                                 CollisionShapes/btOptimizedBvh.h
    98                                 CollisionShapes/btPolyhedralConvexShape.cpp
    99                                 CollisionShapes/btPolyhedralConvexShape.h
    100                                 CollisionShapes/btScaledBvhTriangleMeshShape.cpp
    101                                 CollisionShapes/btScaledBvhTriangleMeshShape.h
    102                                 CollisionShapes/btTetrahedronShape.cpp
    103                                 CollisionShapes/btTetrahedronShape.h
    104                                 CollisionShapes/btSphereShape.cpp
    105                                 CollisionShapes/btSphereShape.h
    106                                 CollisionShapes/btShapeHull.h
    107                                 CollisionShapes/btShapeHull.cpp
    108                                 CollisionShapes/btStaticPlaneShape.cpp
    109                                 CollisionShapes/btStaticPlaneShape.h
    110                                 CollisionShapes/btStridingMeshInterface.cpp
    111                                 CollisionShapes/btStridingMeshInterface.h
    112                                 CollisionShapes/btTriangleCallback.cpp
    113                                 CollisionShapes/btTriangleCallback.h
    114                                 CollisionShapes/btTriangleBuffer.cpp
    115                                 CollisionShapes/btTriangleBuffer.h
    116                                 CollisionShapes/btTriangleIndexVertexArray.cpp
    117                                 CollisionShapes/btTriangleIndexVertexArray.h
    118                                 CollisionShapes/btTriangleIndexVertexMaterialArray.h
    119                                 CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
    120                                 CollisionShapes/btTriangleMesh.cpp
    121                                 CollisionShapes/btTriangleMesh.h
    122                                 CollisionShapes/btTriangleMeshShape.cpp
    123                                 CollisionShapes/btTriangleMeshShape.h
    124                                 CollisionShapes/btUniformScalingShape.cpp
    125                                 CollisionShapes/btUniformScalingShape.h
    126                                 NarrowPhaseCollision/btContinuousConvexCollision.cpp
    127                                 NarrowPhaseCollision/btContinuousConvexCollision.h
    128                                 NarrowPhaseCollision/btGjkEpa.cpp
    129                                 NarrowPhaseCollision/btGjkEpa.h
    130                                 NarrowPhaseCollision/btGjkEpa2.cpp
    131                                 NarrowPhaseCollision/btGjkEpa2.h
    132                                 NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
    133                                 NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
    134                                 NarrowPhaseCollision/btConvexCast.cpp
    135                                 NarrowPhaseCollision/btConvexCast.h
    136                                 NarrowPhaseCollision/btGjkConvexCast.cpp
    137                                 NarrowPhaseCollision/btGjkConvexCast.h
    138                                 NarrowPhaseCollision/btGjkPairDetector.cpp
    139                                 NarrowPhaseCollision/btGjkPairDetector.h
    140                                 NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
    141                                 NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
    142                                 NarrowPhaseCollision/btPersistentManifold.cpp
    143                                 NarrowPhaseCollision/btPersistentManifold.h
    144                                 NarrowPhaseCollision/btRaycastCallback.cpp
    145                                 NarrowPhaseCollision/btRaycastCallback.h
    146                                 NarrowPhaseCollision/btSubSimplexConvexCast.cpp
    147                                 NarrowPhaseCollision/btSubSimplexConvexCast.h
    148                                 NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
    149                                 NarrowPhaseCollision/btVoronoiSimplexSolver.h
     1SET(BulletCollision_SRCS
     2        BroadphaseCollision/btAxisSweep3.cpp
     3        BroadphaseCollision/btBroadphaseProxy.cpp
     4        BroadphaseCollision/btCollisionAlgorithm.cpp
     5        BroadphaseCollision/btDispatcher.cpp
     6        BroadphaseCollision/btDbvtBroadphase.cpp
     7        BroadphaseCollision/btDbvt.cpp
     8        BroadphaseCollision/btMultiSapBroadphase.cpp
     9        BroadphaseCollision/btOverlappingPairCache.cpp
     10        BroadphaseCollision/btQuantizedBvh.cpp
     11        BroadphaseCollision/btSimpleBroadphase.cpp
     12        CollisionDispatch/btActivatingCollisionAlgorithm.cpp
     13        CollisionDispatch/btCollisionDispatcher.cpp
     14        CollisionDispatch/btCollisionObject.cpp
     15        CollisionDispatch/btCollisionWorld.cpp
     16        CollisionDispatch/btCompoundCollisionAlgorithm.cpp
     17        CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
     18        CollisionDispatch/btDefaultCollisionConfiguration.cpp
     19        CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
     20        CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
     21        CollisionDispatch/btBoxBoxDetector.cpp
     22        CollisionDispatch/btGhostObject.cpp
     23        CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
     24        CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
     25        CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
     26        CollisionDispatch/btConvexConvexAlgorithm.cpp
     27        CollisionDispatch/btEmptyCollisionAlgorithm.cpp
     28        CollisionDispatch/btManifoldResult.cpp
     29        CollisionDispatch/btSimulationIslandManager.cpp
     30        CollisionDispatch/btUnionFind.cpp
     31        CollisionDispatch/SphereTriangleDetector.cpp
     32        CollisionShapes/btBoxShape.cpp
     33        CollisionShapes/btBvhTriangleMeshShape.cpp
     34        CollisionShapes/btCapsuleShape.cpp
     35        CollisionShapes/btCollisionShape.cpp
     36        CollisionShapes/btCompoundShape.cpp
     37        CollisionShapes/btConcaveShape.cpp
     38        CollisionShapes/btConeShape.cpp
     39        CollisionShapes/btConvexHullShape.cpp
     40        CollisionShapes/btConvexPointCloudShape.cpp
     41        CollisionShapes/btConvexShape.cpp
     42        CollisionShapes/btConvexInternalShape.cpp
     43        CollisionShapes/btConvexTriangleMeshShape.cpp
     44        CollisionShapes/btCylinderShape.cpp
     45        CollisionShapes/btEmptyShape.cpp
     46        CollisionShapes/btHeightfieldTerrainShape.cpp
     47        CollisionShapes/btMinkowskiSumShape.cpp
     48        CollisionShapes/btMultimaterialTriangleMeshShape.cpp
     49        CollisionShapes/btMultiSphereShape.cpp
     50        CollisionShapes/btOptimizedBvh.cpp
     51        CollisionShapes/btPolyhedralConvexShape.cpp
     52        CollisionShapes/btScaledBvhTriangleMeshShape.cpp
     53        CollisionShapes/btTetrahedronShape.cpp
     54        CollisionShapes/btSphereShape.cpp
     55        CollisionShapes/btShapeHull.cpp
     56        CollisionShapes/btStaticPlaneShape.cpp
     57        CollisionShapes/btStridingMeshInterface.cpp
     58        CollisionShapes/btTriangleCallback.cpp
     59        CollisionShapes/btTriangleBuffer.cpp
     60        CollisionShapes/btTriangleIndexVertexArray.cpp
     61        CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
     62        CollisionShapes/btTriangleMesh.cpp
     63        CollisionShapes/btTriangleMeshShape.cpp
     64        CollisionShapes/btUniformScalingShape.cpp
     65        Gimpact/btContactProcessing.cpp
     66        Gimpact/btGImpactShape.cpp
     67        Gimpact/gim_contact.cpp
     68        Gimpact/btGImpactBvh.cpp
     69        Gimpact/btGenericPoolAllocator.cpp
     70        Gimpact/gim_memory.cpp
     71        Gimpact/btGImpactCollisionAlgorithm.cpp
     72        Gimpact/btTriangleShapeEx.cpp
     73        Gimpact/gim_tri_collision.cpp
     74        Gimpact/btGImpactQuantizedBvh.cpp
     75        Gimpact/gim_box_set.cpp
     76        NarrowPhaseCollision/btContinuousConvexCollision.cpp
     77        NarrowPhaseCollision/btGjkEpa2.cpp
     78        NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
     79        NarrowPhaseCollision/btConvexCast.cpp
     80        NarrowPhaseCollision/btGjkConvexCast.cpp
     81        NarrowPhaseCollision/btGjkPairDetector.cpp
     82        NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
     83        NarrowPhaseCollision/btPersistentManifold.cpp
     84        NarrowPhaseCollision/btRaycastCallback.cpp
     85        NarrowPhaseCollision/btSubSimplexConvexCast.cpp
     86        NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
    15087)
     88
     89ADD_LIBRARY(BulletCollision ${BulletCollision_SRCS})

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

    r2192 r2430  
    2020
    2121
    22 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
     22SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
    2323:m_sphere(sphere),
    24 m_triangle(triangle)
     24m_triangle(triangle),
     25m_contactBreakingThreshold(contactBreakingThreshold)
    2526{
    2627
    … …  
    4142        btTransform     sphereInTr = transformB.inverseTimes(transformA);
    4243
    43         if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
     44        if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
    4445        {
    4546                if (swapResults)
    … …  
    9495
    9596///combined discrete/continuous sphere-triangle
    96 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
     97bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
    9798{
    9899
    … …  
    116117        }
    117118
    118         ///todo: move this gContactBreakingThreshold into a proper structure
    119         extern btScalar gContactBreakingThreshold;
    120 
    121         btScalar contactMargin = gContactBreakingThreshold;
     119        btScalar contactMargin = contactBreakingThreshold;
    122120        bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
    123121        bool isInsideShellPlane = distanceFromPlane < r;
    … …  
    141139                        for (int i = 0; i < m_triangle->getNumEdges(); i++) {
    142140                               
    143                                 btPoint3 pa;
    144                                 btPoint3 pb;
     141                                btVector3 pa;
     142                                btVector3 pb;
    145143                               
    146144                                m_triangle->getEdge(i,pa,pb);

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

    r2192 r2430  
    1818
    1919#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
    20 #include "LinearMath/btPoint3.h"
     20
    2121
    2222
    … …  
    3131        virtual void    getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
    3232
    33         SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle);
     33        SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold);
    3434
    3535        virtual ~SphereTriangleDetector() {};
    … …  
    3737private:
    3838
    39         bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact);
     39        bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
    4040        bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
    4141        bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
    … …  
    4343        btSphereShape* m_sphere;
    4444        btTriangleShape* m_triangle;
    45 
     45        btScalar        m_contactBreakingThreshold;
    4646       
    4747};

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

    r2192 r2430  
    2323
    2424btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
    25 : btCollisionAlgorithm(ci),
     25: btActivatingCollisionAlgorithm(ci,obj0,obj1),
    2626m_ownManifold(false),
    2727m_manifoldPtr(mf)

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

    r2192 r2430  
    1717#define BOX_BOX__COLLISION_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
    2121#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
    … …  
    2525
    2626///box-box collision detection
    27 class btBoxBoxCollisionAlgorithm : public btCollisionAlgorithm
     27class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
    2828{
    2929        bool    m_ownManifold;
    … …  
    3232public:
    3333        btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
    34                 : btCollisionAlgorithm(ci) {}
     34                : btActivatingCollisionAlgorithm(ci) {}
    3535
    3636        virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

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

    r2192 r2430  
    208208    }
    209209    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
    210     a = 1.f/(btScalar(3.0)*(a+q));
     210        if (btFabs(a+q) > SIMD_EPSILON)
     211        {
     212                a = 1.f/(btScalar(3.0)*(a+q));
     213        } else
     214        {
     215                a=1e30f;
     216        }
    211217    cx = a*(cx + q*(p[n*2-2]+p[0]));
    212218    cy = a*(cy + q*(p[n*2-1]+p[1]));

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

    r2192 r2430  
    2323///btCollisionConfiguration allows to configure Bullet collision detection
    2424///stack allocator size, default collision algorithms and persistent manifold pool size
    25 ///todo: describe the meaning
     25///@todo: describe the meaning
    2626class   btCollisionConfiguration
    2727{

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

    r2192 r2430  
    1818
    1919#include "LinearMath/btAlignedObjectArray.h"
    20 typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
    2120class btCollisionAlgorithm;
    2221class btCollisionObject;

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

    r2192 r2430  
    5858       
    5959       
    60 };
     60}
    6161
    6262
    … …  
    7979        btCollisionObject* body0 = (btCollisionObject*)b0;
    8080        btCollisionObject* body1 = (btCollisionObject*)b1;
     81
     82        btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
    8183       
    8284        void* mem = 0;
    … …  
    9092
    9193        }
    92         btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
     94        btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold);
    9395        manifold->m_index1a = m_manifoldsPtr.size();
    9496        m_manifoldsPtr.push_back(manifold);

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

    r2192 r2430  
    3030#include "LinearMath/btMotionState.h"
    3131#include "LinearMath/btAlignedAllocator.h"
    32 
     32#include "LinearMath/btAlignedObjectArray.h"
     33
     34
     35typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
    3336
    3437
    … …  
    7275        void*                   m_userObjectPointer;
    7376
    74         ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody etc.
     77        ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
    7578        ///do not assign your own m_internalType unless you write a new dynamics object class.
    7679        int                             m_internalType;
    … …  
    104107                CF_KINEMATIC_OBJECT= 2,
    105108                CF_NO_CONTACT_RESPONSE = 4,
    106                 CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution)
     109                CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
     110                CF_CHARACTER_OBJECT = 16
    107111        };
    108112
    … …  
    111115                CO_COLLISION_OBJECT =1,
    112116                CO_RIGID_BODY,
    113                 CO_SOFT_BODY
     117                CO_SOFT_BODY,
     118                ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
     119                ///It is useful for collision sensors, explosion objects, character controller etc.
     120                CO_GHOST_OBJECT
    114121        };
    115122
    … …  
    177184        }
    178185
    179         int     getActivationState() const { return m_activationState1;}
     186        SIMD_FORCE_INLINE       int     getActivationState() const { return m_activationState1;}
    180187       
    181188        void setActivationState(int newState);
    … …  
    194201        void    activate(bool forceActivation = false);
    195202
    196         inline bool isActive() const
     203        SIMD_FORCE_INLINE bool isActive() const
    197204        {
    198205                return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
    … …  
    238245
    239246
    240         btBroadphaseProxy*      getBroadphaseHandle()
     247        SIMD_FORCE_INLINE btBroadphaseProxy*    getBroadphaseHandle()
    241248        {
    242249                return m_broadphaseHandle;
    243250        }
    244251
    245         const btBroadphaseProxy*        getBroadphaseHandle() const
     252        SIMD_FORCE_INLINE const btBroadphaseProxy*      getBroadphaseHandle() const
    246253        {
    247254                return m_broadphaseHandle;
    … …  
    289296        }
    290297
    291         const int getIslandTag() const
     298        SIMD_FORCE_INLINE int getIslandTag() const
    292299        {
    293300                return  m_islandTag1;
    … …  
    299306        }
    300307
    301         const int getCompanionId() const
     308        SIMD_FORCE_INLINE int getCompanionId() const
    302309        {
    303310                return  m_companionId;
    … …  
    309316        }
    310317
    311         const btScalar                  getHitFraction() const
     318        SIMD_FORCE_INLINE btScalar                      getHitFraction() const
    312319        {
    313320                return m_hitFraction;
    … …  
    320327
    321328       
    322         const int       getCollisionFlags() const
     329        SIMD_FORCE_INLINE int   getCollisionFlags() const
    323330        {
    324331                return m_collisionFlags;

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

    r2192 r2430  
    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
    3538
    3639//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
    … …  
    114117}
    115118
     119
     120
    116121void    btCollisionWorld::updateAabbs()
    117122{
    … …  
    126131                if (colObj->isActive())
    127132                {
    128                         btPoint3 minAabb,maxAabb;
     133                        btVector3 minAabb,maxAabb;
    129134                        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
    130135                        //need to increase the aabb for contact thresholds
    … …  
    227232        if (collisionShape->isConvex())
    228233        {
     234//              BT_PROFILE("rayTestConvex");
    229235                btConvexCast::CastResult castResult;
    230236                castResult.m_fraction = resultCallback.m_closestHitFraction;
    … …  
    270276                if (collisionShape->isConcave())
    271277                {
     278//                      BT_PROFILE("rayTestConcave");
    272279                        if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
    273280                        {
    … …  
    318325                        } else
    319326                        {
    320                                 btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape;
     327                                //generic (slower) case
     328                                btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
    321329
    322330                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
    … …  
    331339                                        btCollisionWorld::RayResultCallback* m_resultCallback;
    332340                                        btCollisionObject*      m_collisionObject;
    333                                         btTriangleMeshShape*    m_triangleMesh;
     341                                        btConcaveShape* m_triangleMesh;
    334342
    335343                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
    336                                                 btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
     344                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
    337345                                                btTriangleRaycastCallback(from,to),
    338346                                                        m_resultCallback(resultCallback),
    … …  
    364372
    365373
    366                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
     374                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
    367375                                rcb.m_hitFraction = resultCallback.m_closestHitFraction;
    368376
    … …  
    372380                                rayAabbMaxLocal.setMax(rayToLocal);
    373381
    374                                 triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
     382                                concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
    375383                        }
    376384                } else {
    377                         //todo: use AABB tree or other BVH acceleration structure!
     385//                      BT_PROFILE("rayTestCompound");
     386                        ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
    378387                        if (collisionShape->isCompound())
    379388                        {
    … …  
    409418        if (collisionShape->isConvex())
    410419        {
     420                //BT_PROFILE("convexSweepConvex");
    411421                btConvexCast::CastResult castResult;
    412422                castResult.m_allowedPenetration = allowedPenetration;
    413                 castResult.m_fraction = btScalar(1.);//??
     423                castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
    414424
    415425                btConvexShape* convexShape = (btConvexShape*) collisionShape;
    … …  
    453463                        if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
    454464                        {
     465                                //BT_PROFILE("convexSweepbtBvhTriangleMesh");
    455466                                btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
    456467                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
    … …  
    509520                        } else
    510521                        {
    511                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
     522                                //BT_PROFILE("convexSweepConcave");
     523                                btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
    512524                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
    513525                                btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
    … …  
    521533                                        btCollisionWorld::ConvexResultCallback* m_resultCallback;
    522534                                        btCollisionObject*      m_collisionObject;
    523                                         btTriangleMeshShape*    m_triangleMesh;
     535                                        btConcaveShape* m_triangleMesh;
    524536
    525537                                        BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
    526                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
     538                                                btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*      triangleMesh, const btTransform& triangleToWorld):
    527539                                                btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
    528540                                                        m_resultCallback(resultCallback),
    … …  
    557569                                };
    558570
    559                                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
     571                                BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
    560572                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
    561573                                btVector3 boxMinLocal, boxMaxLocal;
    … …  
    568580                                rayAabbMinLocal += boxMinLocal;
    569581                                rayAabbMaxLocal += boxMaxLocal;
    570                                 triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
     582                                concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
    571583                        }
    572584                } else {
    573                         //todo: use AABB tree or other BVH acceleration structure!
     585                        ///@todo : use AABB tree or other BVH acceleration structure!
    574586                        if (collisionShape->isCompound())
    575587                        {
     588                                BT_PROFILE("convexSweepCompound");
    576589                                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
    577590                                int i=0;
    … …  
    597610}
    598611
    599 void    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++)
     612
     613struct btSingleRayCallback : public btBroadphaseRayCallback
     614{
     615
     616        btVector3       m_rayFromWorld;
     617        btVector3       m_rayToWorld;
     618        btTransform     m_rayFromTrans;
     619        btTransform     m_rayToTrans;
     620        btVector3       m_hitNormal;
     621
     622        const btCollisionWorld* m_world;
     623        btCollisionWorld::RayResultCallback&    m_resultCallback;
     624
     625        btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
     626        :m_rayFromWorld(rayFromWorld),
     627        m_rayToWorld(rayToWorld),
     628        m_world(world),
     629        m_resultCallback(resultCallback)
     630        {
     631                m_rayFromTrans.setIdentity();
     632                m_rayFromTrans.setOrigin(m_rayFromWorld);
     633                m_rayToTrans.setIdentity();
     634                m_rayToTrans.setOrigin(m_rayToWorld);
     635
     636                btVector3 rayDir = (rayToWorld-rayFromWorld);
     637
     638                rayDir.normalize ();
     639                ///what about division by zero? --> just set rayDirection[i] to INF/1e30
     640                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
     641                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
     642                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
     643                m_signs[0] = m_rayDirectionInverse[0] < 0.0;
     644                m_signs[1] = m_rayDirectionInverse[1] < 0.0;
     645                m_signs[2] = m_rayDirectionInverse[2] < 0.0;
     646
     647                m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
     648
     649        }
     650
     651       
     652
     653        virtual bool    process(const btBroadphaseProxy* proxy)
    614654        {
    615655                ///terminate further ray tests, once the closestHitFraction reached zero
    616                 if (resultCallback.m_closestHitFraction == btScalar(0.f))
    617                         break;
    618 
    619                 btCollisionObject*      collisionObject= m_collisionObjects[i];
     656                if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
     657                        return false;
     658
     659                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
     660
    620661                //only perform raycast if filterMask matches
    621                 if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
     662                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
     663                {
    622664                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
     665                        //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
     666                       
     667#ifdef RECALCULATE_AABB
    623668                        btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    624669                        collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
    625 
    626                         btScalar hitLambda = resultCallback.m_closestHitFraction;
    627                         btVector3 hitNormal;
    628                         if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
    629                         {
    630                                 rayTestSingle(rayFromTrans,rayToTrans,
     670#else
     671                        //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
     672                        const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
     673                        const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
     674#endif
     675                        //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
     676                        //culling already done by broadphase
     677                        //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
     678                        {
     679                                m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
    631680                                        collisionObject,
    632681                                                collisionObject->getCollisionShape(),
    633682                                                collisionObject->getWorldTransform(),
    634                                                 resultCallback);
     683                                                m_resultCallback);
    635684                        }
    636685                }
    637 
    638         }
    639 
    640 }
    641 
    642 void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback) const
    643 {
     686                return true;
     687        }
     688};
     689
     690void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
     691{
     692        BT_PROFILE("rayTest");
     693        /// use the broadphase to accelerate the search for objects, based on their aabb
     694        /// and for each object with ray-aabb overlap, perform an exact ray test
     695        btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
     696
     697#ifndef USE_BRUTEFORCE_RAYBROADPHASE
     698        m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
     699#else
     700        for (int i=0;i<this->getNumCollisionObjects();i++)
     701        {
     702                rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
     703        }       
     704#endif //USE_BRUTEFORCE_RAYBROADPHASE
     705
     706}
     707
     708
     709struct btSingleSweepCallback : public btBroadphaseRayCallback
     710{
     711
     712        btTransform     m_convexFromTrans;
     713        btTransform     m_convexToTrans;
     714        btVector3       m_hitNormal;
     715        const btCollisionWorld* m_world;
     716        btCollisionWorld::ConvexResultCallback& m_resultCallback;
     717        btScalar        m_allowedCcdPenetration;
     718        const btConvexShape* m_castShape;
     719
     720
     721        btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
     722                :m_convexFromTrans(convexFromTrans),
     723                m_convexToTrans(convexToTrans),
     724                m_world(world),
     725                m_resultCallback(resultCallback),
     726                m_allowedCcdPenetration(allowedPenetration),
     727                m_castShape(castShape)
     728        {
     729                btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
     730                btVector3 rayDir = unnormalizedRayDir.normalized();
     731                ///what about division by zero? --> just set rayDirection[i] to INF/1e30
     732                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
     733                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
     734                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
     735                m_signs[0] = m_rayDirectionInverse[0] < 0.0;
     736                m_signs[1] = m_rayDirectionInverse[1] < 0.0;
     737                m_signs[2] = m_rayDirectionInverse[2] < 0.0;
     738
     739                m_lambda_max = rayDir.dot(unnormalizedRayDir);
     740
     741        }
     742
     743        virtual bool    process(const btBroadphaseProxy* proxy)
     744        {
     745                ///terminate further convex sweep tests, once the closestHitFraction reached zero
     746                if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
     747                        return false;
     748
     749                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
     750
     751                //only perform raycast if filterMask matches
     752                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
     753                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
     754                        m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
     755                                        collisionObject,
     756                                                collisionObject->getCollisionShape(),
     757                                                collisionObject->getWorldTransform(),
     758                                                m_resultCallback,
     759                                                m_allowedCcdPenetration);
     760                }
     761               
     762                return true;
     763        }
     764};
     765
     766
     767
     768void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
     769{
     770
     771        BT_PROFILE("convexSweepTest");
     772        /// use the broadphase to accelerate the search for objects, based on their aabb
     773        /// and for each object with ray-aabb overlap, perform an exact ray test
     774        /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
     775
     776       
     777
    644778        btTransform     convexFromTrans,convexToTrans;
    645779        convexFromTrans = convexFromWorld;
    … …  
    650784                btVector3 linVel, angVel;
    651785                btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
     786                btVector3 zeroLinVel;
     787                zeroLinVel.setValue(0,0,0);
    652788                btTransform R;
    653789                R.setIdentity ();
    654790                R.setRotation (convexFromTrans.getRotation());
    655                 castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
    656         }
    657 
     791                castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
     792        }
     793
     794#ifndef USE_BRUTEFORCE_RAYBROADPHASE
     795
     796        btSingleSweepCallback   convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
     797
     798        m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
     799
     800#else
    658801        /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
    659802        // do a ray-shape query using convexCaster (CCD)
    … …  
    677820                                                collisionObject->getWorldTransform(),
    678821                                                resultCallback,
    679                                                 getDispatchInfo().m_allowedCcdPenetration);
     822                                                allowedCcdPenetration);
    680823                        }
    681824                }
    682825        }
    683 
    684 }
     826#endif //USE_BRUTEFORCE_RAYBROADPHASE
     827}

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

    r2192 r2430  
    11/*
    22Bullet Continuous Collision Detection and Physics Library
    3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
     3Copyright (c) 2003-2006 Erwin Coumans  http://bulletphysics.com/Bullet/
    44
    55This software is provided 'as-is', without any express or implied warranty.
    … …  
    2323 * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
    2424 *
    25  * There is the Physics Forum for Feedback and bteral Collision Detection and Physics discussions.
    26  * Please visit http://www.continuousphysics.com/Bullet/phpBB2/index.php
     25 * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
     26 * Please visit http://www.bulletphysics.com
    2727 *
    2828 * @section install_sec Installation
    2929 *
    3030 * @subsection step1 Step 1: Download
    31  * You can download the Bullet Physics Library from our website: http://www.continuousphysics.com/Bullet/
     31 * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
    3232 * @subsection step2 Step 2: Building
    3333 * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8.
    3434 * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version).
    3535 *
    36  * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org, or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
     36 * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
    3737 * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam.
    3838 * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files.
    39  * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/pub/jam/
     39 * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam
    4040 *
    4141 * @subsection step3 Step 3: Testing demos
    … …  
    7272#include "LinearMath/btTransform.h"
    7373#include "btCollisionObject.h"
    74 #include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray
     74#include "btCollisionDispatcher.h"
    7575#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
    7676#include "LinearMath/btAlignedObjectArray.h"
    … …  
    108108        }
    109109
     110        const btBroadphaseInterface*    getBroadphase() const
     111        {
     112                return m_broadphasePairCache;
     113        }
     114
    110115        btBroadphaseInterface*  getBroadphase()
    111116        {
    … …  
    131136        virtual void    updateAabbs();
    132137
     138       
    133139        virtual void    setDebugDrawer(btIDebugDraw*    debugDrawer)
    134140        {
    … …  
    348354        // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
    349355        // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
    350         void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback) const;
     356        void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback,  btScalar allowedCcdPenetration = btScalar(0.)) const;
    351357
    352358

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

    r2192 r2430  
    2222
    2323btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
    24 :btCollisionAlgorithm(ci),
     24:btActivatingCollisionAlgorithm(ci,body0,body1),
    2525m_isSwapped(isSwapped),
    2626m_sharedManifold(ci.m_manifold)

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

    r2192 r2430  
    1717#define COMPOUND_COLLISION_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
    2121#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
    … …  
    2929
    3030/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
    31 class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
     31class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
    3232{
    3333        btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;

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

    r2192 r2430  
    2828
    2929btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
    30 : btCollisionAlgorithm(ci),
     30: btActivatingCollisionAlgorithm(ci,body0,body1),
    3131m_isSwapped(isSwapped),
    3232m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
    … …  
    7373{
    7474        m_dispatcher->clearManifold(m_manifoldPtr);
    75 };
     75}
    7676
    7777
    … …  
    9494       
    9595        ///debug drawing of the overlapping triangles
    96         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
     96        if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
    9797        {
    9898                btVector3 color(255,255,0);

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

    r2192 r2430  
    1717#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
    2121#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
    … …  
    3535        btVector3       m_aabbMax ;
    3636
     37
    3738        btManifoldResult* m_resultOut;
    38 
    3939        btDispatcher*   m_dispatcher;
    4040        const btDispatcherInfo* m_dispatchInfoPtr;
    … …  
    7171
    7272/// btConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
    73 class btConvexConcaveCollisionAlgorithm  : public btCollisionAlgorithm
     73class btConvexConcaveCollisionAlgorithm  : public btActivatingCollisionAlgorithm
    7474{
    7575
    … …  
    7777
    7878        btConvexTriangleCallback m_btConvexTriangleCallback;
     79
    7980
    8081

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

    r2192 r2430  
    3939#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
    4040
    41 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
     41#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
    4242#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
    4343
    … …  
    6161
    6262btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
    63 : btCollisionAlgorithm(ci),
    64 m_gjkPairDetector(0,0,simplexSolver,pdSolver),
     63: btActivatingCollisionAlgorithm(ci,body0,body1),
     64m_simplexSolver(simplexSolver),
     65m_pdSolver(pdSolver),
    6566m_ownManifold (false),
    6667m_manifoldPtr(mf),
    6768m_lowLevelOfDetail(false)
     69#ifdef USE_SEPDISTANCE_UTIL2
     70,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
     71                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
     72#endif
    6873{
    6974        (void)body0;
    7075        (void)body1;
    71 
    72 
    7376}
    7477
    … …  
    108111        resultOut->setPersistentManifold(m_manifoldPtr);
    109112
    110 #ifdef USE_BT_GJKEPA
    111         btConvexShape*                          shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
    112         btConvexShape*                          shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
    113         const btScalar                          radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
    114         btGjkEpaSolver::sResults        results;
    115         if(btGjkEpaSolver::Collide(     shape0,body0->getWorldTransform(),
    116                                                                 shape1,body1->getWorldTransform(),
    117                                                                 radialmargin,results))
    118                 {
    119                 dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
    120                 resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
    121                 }
    122 #else
     113       
    123114
    124115        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
    125116        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
     117
     118#ifdef USE_SEPDISTANCE_UTIL2
     119        m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
     120        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
     121#endif //USE_SEPDISTANCE_UTIL2
     122
     123        {
     124
    126125       
    127126        btGjkPairDetector::ClosestPointInput input;
    128127
     128        btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
    129129        //TODO: if (dispatchInfo.m_useContinuous)
    130         m_gjkPairDetector.setMinkowskiA(min0);
    131         m_gjkPairDetector.setMinkowskiB(min1);
    132         input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
    133         input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
     130        gjkPairDetector.setMinkowskiA(min0);
     131        gjkPairDetector.setMinkowskiB(min1);
     132
     133#ifdef USE_SEPDISTANCE_UTIL2
     134        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
     135        {
     136                input.m_maximumDistanceSquared = 1e30f;
     137        } else
     138#endif //USE_SEPDISTANCE_UTIL2
     139        {
     140                input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
     141                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
     142        }
     143
    134144        input.m_stackAlloc = dispatchInfo.m_stackAllocator;
    135 
    136 //      input.m_maximumDistanceSquared = btScalar(1e30);
    137        
    138145        input.m_transformA = body0->getWorldTransform();
    139146        input.m_transformB = body1->getWorldTransform();
    140        
    141         m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
    142 #endif
     147
     148        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
     149
     150        btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
     151
     152#ifdef USE_SEPDISTANCE_UTIL2
     153        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
     154        {
     155                m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
     156        }
     157#endif //USE_SEPDISTANCE_UTIL2
     158
     159
     160        }
    143161
    144162        if (m_ownManifold)

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

    r2192 r2430  
    1717#define CONVEX_CONVEX_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
    2121#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
    … …  
    2424#include "btCollisionCreateFunc.h"
    2525#include "btCollisionDispatcher.h"
     26#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
    2627
    2728class btConvexPenetrationDepthSolver;
    2829
     30///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise
     31///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions.
     32///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util
     33///for certain pairs that have a small size ratio
     34///#define USE_SEPDISTANCE_UTIL2 1
     35
    2936///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
    30 class btConvexConvexAlgorithm : public btCollisionAlgorithm
     37class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
    3138{
    32         btGjkPairDetector m_gjkPairDetector;
    33 public:
     39#ifdef USE_SEPDISTANCE_UTIL2
     40        btConvexSeparatingDistanceUtil  m_sepDistance;
     41#endif
     42        btSimplexSolverInterface*               m_simplexSolver;
     43        btConvexPenetrationDepthSolver* m_pdSolver;
    3444
     45       
    3546        bool    m_ownManifold;
    3647        btPersistentManifold*   m_manifoldPtr;
    3748        bool                    m_lowLevelOfDetail;
     49       
     50        ///cache separating vector to speedup collision detection
    3851       
    3952

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

    r2192 r2430  
    101101        int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
    102102        int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
    103         int maxSize4 = sizeof(btEmptyAlgorithm);
    104        
     103        int sl = sizeof(btConvexSeparatingDistanceUtil);
     104        sl = sizeof(btGjkPairDetector);
    105105        int     collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2);
    106106        collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
    107         collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
    108107
    109108        if (constructionInfo.m_stackAlloc)

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

    r2192 r2430  
    3434                m_persistentManifoldPool(0),
    3535                m_collisionAlgorithmPool(0),
    36                 m_defaultMaxPersistentManifoldPoolSize(65535),
    37                 m_defaultMaxCollisionAlgorithmPoolSize(65535),
    38                 m_defaultStackAllocatorSize(5*1024*1024)
     36                m_defaultMaxPersistentManifoldPoolSize(4096),
     37                m_defaultMaxCollisionAlgorithmPoolSize(4096),
     38                m_defaultStackAllocatorSize(0)
    3939        {
    4040        }
    … …  
    4545///btCollisionConfiguration allows to configure Bullet collision detection
    4646///stack allocator, pool memory allocators
    47 ///todo: describe the meaning
     47///@todo: describe the meaning
    4848class   btDefaultCollisionConfiguration : public btCollisionConfiguration
    4949{

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

    r2192 r2430  
    9494   newPt.m_index1  = m_index1;
    9595       
    96         ///todo, check this for any side effects
     96        ///@todo, check this for any side effects
    9797        if (insertIndex >= 0)
    9898        {

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

    r2192 r2430  
    144144
    145145
    146 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects)
     146void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
    147147{
    148148
    149149        BT_PROFILE("islandUnionFindAndQuickSort");
    150150       
     151        btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
     152
    151153        m_islandmanifold.resize(0);
    152154
    … …  
    239241                                        {
    240242                                                colObj0->setActivationState( WANTS_DEACTIVATION);
     243                                                colObj0->setDeactivationTime(0.f);
    241244                                        }
    242245                                }
    … …  
    263266                 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
    264267               
    265                  //todo: check sleeping conditions!
     268                 ///@todo: check sleeping conditions!
    266269                 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
    267270                        ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
    … …  
    288291
    289292
    290 //
    291 // todo: this is random access, it can be walked 'cache friendly'!
    292 //
    293 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
    294 {
    295 
    296         buildIslands(dispatcher,collisionObjects);
     293///@todo: this is random access, it can be walked 'cache friendly'!
     294void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
     295{
     296        btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
     297
     298        buildIslands(dispatcher,collisionWorld);
    297299
    298300        int endIslandIndex=1;

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

    r2192 r2430  
    2020#include "btCollisionCreateFunc.h"
    2121#include "LinearMath/btAlignedObjectArray.h"
    22 
     22#include "btCollisionObject.h"
    2323
    2424class btCollisionObject;
    … …  
    6262        };
    6363
    64         void    buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);
     64        void    buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback);
    6565
    66         void buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects);
     66        void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
    6767
    6868};

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

    r2192 r2430  
    2222
    2323btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
    24 : btCollisionAlgorithm(ci),
     24: btActivatingCollisionAlgorithm(ci,col0,col1),
    2525m_ownManifold(false),
    2626m_manifoldPtr(mf),

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

    r2192 r2430  
    1717#define SPHERE_BOX_COLLISION_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
    2121#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
    … …  
    2727/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
    2828/// Other features are frame-coherency (persistent data) and collision response.
    29 class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm
     29class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
    3030{
    3131        bool    m_ownManifold;

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

    r2192 r2430  
    2020
    2121btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
    22 : btCollisionAlgorithm(ci),
     22: btActivatingCollisionAlgorithm(ci,col0,col1),
    2323m_ownManifold(false),
    2424m_manifoldPtr(mf)

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

    r2192 r2430  
    1717#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
    2121#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
    … …  
    2727/// Other features are frame-coherency (persistent data) and collision response.
    2828/// Also provides the most basic sample for custom/user btCollisionAlgorithm
    29 class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
     29class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
    3030{
    3131        bool    m_ownManifold;
    … …  
    3636
    3737        btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
    38                 : btCollisionAlgorithm(ci) {}
     38                : btActivatingCollisionAlgorithm(ci) {}
    3939
    4040        virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

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

    r2192 r2430  
    2323
    2424btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
    25 : btCollisionAlgorithm(ci),
     25: btActivatingCollisionAlgorithm(ci,col0,col1),
    2626m_ownManifold(false),
    2727m_manifoldPtr(mf),
    … …  
    5757        /// report a contact. internally this will be kept persistent, and contact reduction is done
    5858        resultOut->setPersistentManifold(m_manifoldPtr);
    59         SphereTriangleDetector detector(sphere,triangle);
     59        SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
    6060       
    6161        btDiscreteCollisionDetectorInterface::ClosestPointInput input;
    62         input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds
     62        input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds
    6363        input.m_transformA = sphereObj->getWorldTransform();
    6464        input.m_transformB = triObj->getWorldTransform();

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

    r2192 r2430  
    1717#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
    1818
    19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     19#include "btActivatingCollisionAlgorithm.h"
    2020#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
    2121#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
    … …  
    2626/// Other features are frame-coherency (persistent data) and collision response.
    2727/// Also provides the most basic sample for custom/user btCollisionAlgorithm
    28 class btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm
     28class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
    2929{
    3030        bool    m_ownManifold;
    … …  
    3636
    3737        btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
    38                 : btCollisionAlgorithm(ci) {}
     38                : btActivatingCollisionAlgorithm(ci) {}
    3939
    4040        virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

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

    r2192 r2430  
    2020#include "btCollisionMargin.h"
    2121#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
    22 #include "LinearMath/btPoint3.h"
     22#include "LinearMath/btVector3.h"
    2323#include "LinearMath/btMinMax.h"
    2424
    … …  
    118118        virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
    119119
    120         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const
     120        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
    121121        {
    122122                //this plane might not be aligned...
    … …  
    191191
    192192       
    193         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const
     193        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
    194194        //virtual void getEdge(int i,Edge& edge) const
    195195        {
    … …  
    262262
    263263       
    264         virtual bool isInside(const btPoint3& pt,btScalar tolerance) const
     264        virtual bool isInside(const btVector3& pt,btScalar tolerance) const
    265265        {
    266266                btVector3 halfExtents = getHalfExtentsWithoutMargin();

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

    r2192 r2430  
    144144                        {
    145145                                int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
    146 
    147                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
    148 
    149                                 m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());           
     146                               
     147                                if (type == PHY_FLOAT)
     148                                {
     149                                        float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
     150                                       
     151                                        m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());           
     152                                }
     153                                else
     154                                {
     155                                        double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
     156                                       
     157                                        m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ());             
     158                                }
    150159                        }
    151160
    … …  
    205214                                int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
    206215
    207                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
    208 
    209                                 m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());           
     216                                if (type == PHY_FLOAT)
     217                                {
     218                                        float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
     219
     220                                        m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());           
     221                                }
     222                                else
     223                                {
     224                                        double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
     225                                       
     226                                        m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ());             
     227                                }
    210228                        }
    211229
    … …  
    282300                                printf("%d ,",graphicsindex);
    283301#endif //DEBUG_TRIANGLE_MESH
    284                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
    285 
    286                                 m_triangle[j] = btVector3(
    287                                         graphicsbase[0]*meshScaling.getX(),
    288                                         graphicsbase[1]*meshScaling.getY(),
    289                                         graphicsbase[2]*meshScaling.getZ());
     302                                if (type == PHY_FLOAT)
     303                                {
     304                                        float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
     305                                       
     306                                        m_triangle[j] = btVector3(
     307                                                                                                                                                graphicsbase[0]*meshScaling.getX(),
     308                                                                                                                                                graphicsbase[1]*meshScaling.getY(),
     309                                                                                                                                                graphicsbase[2]*meshScaling.getZ());
     310                                }
     311                                else
     312                                {
     313                                        double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
     314
     315                                        m_triangle[j] = btVector3(
     316                                                btScalar(graphicsbase[0])*meshScaling.getX(),
     317                                                btScalar(graphicsbase[1])*meshScaling.getY(),
     318                                                btScalar(graphicsbase[2])*meshScaling.getZ());
     319                                }
    290320#ifdef DEBUG_TRIANGLE_MESH
    291321                                printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());

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

    r2192 r2430  
    5050                        halfExtents += btVector3(getMargin(),getMargin(),getMargin());
    5151                        btMatrix3x3 abs_b = t.getBasis().absolute(); 
    52                         btPoint3 center = t.getOrigin();
     52                        btVector3 center = t.getOrigin();
    5353                        btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));             
    5454                       

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

    r2192 r2430  
    4343}
    4444
     45btScalar        btCollisionShape::getContactBreakingThreshold() const
     46{
     47        ///@todo make this 0.1 configurable
     48        return getAngularMotionDisc() * btScalar(0.1);
     49}
    4550btScalar        btCollisionShape::getAngularMotionDisc() const
    4651{
     52        ///@todo cache this value, to improve performance
    4753        btVector3       center;
    4854        btScalar disc;
    … …  
    6672        // add linear motion
    6773        btVector3 linMotion = linvel*timeStep;
    68         //todo: simd would have a vector max/min operation, instead of per-element access
     74        ///@todo: simd would have a vector max/min operation, instead of per-element access
    6975        if (linMotion.x() > btScalar(0.))
    7076                temporalAabbMaxx += linMotion.x();

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

    r2192 r2430  
    2020#include "LinearMath/btVector3.h"
    2121#include "LinearMath/btMatrix3x3.h"
    22 #include "LinearMath/btPoint3.h"
    2322#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
    2423
    … …  
    4746        ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations.
    4847        virtual btScalar        getAngularMotionDisc() const;
     48
     49        virtual btScalar        getContactBreakingThreshold() const;
    4950
    5051

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

    r2192 r2430  
    1818#include "BulletCollision/BroadphaseCollision/btDbvt.h"
    1919
    20 btCompoundShape::btCompoundShape()
     20btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
    2121: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)),
    2222m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
    … …  
    2626{
    2727        m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
    28         void* mem = btAlignedAlloc(sizeof(btDbvt),16);
    29         m_dynamicAabbTree = new(mem) btDbvt();
    30         btAssert(mem==m_dynamicAabbTree);
     28
     29        if (enableDynamicAabbTree)
     30        {
     31                void* mem = btAlignedAlloc(sizeof(btDbvt),16);
     32                m_dynamicAabbTree = new(mem) btDbvt();
     33                btAssert(mem==m_dynamicAabbTree);
     34        }
    3135}
    3236
    … …  
    7781}
    7882
     83void    btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform)
     84{
     85        m_children[childIndex].m_transform = newChildTransform;
     86
     87        if (m_dynamicAabbTree)
     88        {
     89                ///update the dynamic aabb tree
     90                btVector3 localAabbMin,localAabbMax;
     91                m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
     92                ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
     93                int index = m_children.size()-1;
     94                m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
     95        }
     96
     97        recalculateLocalAabb();
     98}
     99
    79100void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
    80101{
    … …  
    88109
    89110}
     111
     112
    90113
    91114void btCompoundShape::removeChildShape(btCollisionShape* shape)
    … …  
    100123                        m_children.pop_back();
    101124                        //remove it from the m_dynamicAabbTree too
     125                        //@todo: this leads to problems due to caching in the btCompoundCollisionAlgorithm
     126                        //so effectively, removeChildShape is broken at the moment
    102127                        //m_dynamicAabbTree->remove(m_aabbProxies[i]);
    103128                        //m_aabbProxies.swap(i,m_children.size()-1);
    … …  
    142167        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
    143168
    144         btPoint3 center = trans(localCenter);
     169        btVector3 center = trans(localCenter);
    145170
    146171        btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
    … …  
    181206        btScalar totalMass = 0;
    182207        btVector3 center(0, 0, 0);
    183         for (int k = 0; k < n; k++)
     208        int k;
     209
     210        for (k = 0; k < n; k++)
    184211        {
    185212                center += m_children[k].m_transform.getOrigin() * masses[k];
    … …  
    190217
    191218        btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
    192         for (int k = 0; k < n; k++)
     219        for ( k = 0; k < n; k++)
    193220        {
    194221                btVector3 i;

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

    r2192 r2430  
    4747}
    4848
    49 /// btCompoundShape allows to store multiple other btCollisionShapes
     49/// The btCompoundShape allows to store multiple other btCollisionShapes
    5050/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape.
     51/// It has an (optional) dynamic aabb tree to accelerate early rejection tests.
     52/// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25
     53/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
    5154ATTRIBUTE_ALIGNED16(class) btCompoundShape      : public btCollisionShape
    5255{
    53         //btAlignedObjectArray<btTransform>             m_childTransforms;
    54         //btAlignedObjectArray<btCollisionShape*>       m_childShapes;
    5556        btAlignedObjectArray<btCompoundShapeChild> m_children;
    5657        btVector3                                               m_localAabbMin;
    5758        btVector3                                               m_localAabbMax;
    5859
    59         //btOptimizedBvh*                                       m_aabbTree;
    6060        btDbvt*                                                 m_dynamicAabbTree;
    6161
    … …  
    6363        BT_DECLARE_ALIGNED_ALLOCATOR();
    6464
    65         btCompoundShape();
     65        btCompoundShape(bool enableDynamicAabbTree = true);
    6666
    6767        virtual ~btCompoundShape();
    … …  
    8989        }
    9090
    91         btTransform     getChildTransform(int index)
     91        btTransform&    getChildTransform(int index)
    9292        {
    9393                return m_children[index].m_transform;
    9494        }
    95         const btTransform       getChildTransform(int index) const
     95        const btTransform&      getChildTransform(int index) const
    9696        {
    9797                return m_children[index].m_transform;
    9898        }
     99
     100        ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree)
     101        void    updateChildTransform(int childIndex, const btTransform& newChildTransform);
    99102
    100103

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

    r2192 r2430  
    2121#include "btTriangleCallback.h"
    2222
     23/// PHY_ScalarType enumerates possible scalar types.
     24/// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use
     25typedef enum PHY_ScalarType {
     26        PHY_FLOAT,
     27        PHY_DOUBLE,
     28        PHY_INTEGER,
     29        PHY_SHORT,
     30        PHY_FIXEDPOINT88,
     31        PHY_UCHAR
     32} PHY_ScalarType;
    2333
    2434///The btConcaveShape class provides an interface for non-moving (static) concave shapes.

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

    r2192 r2430  
    1515
    1616#include "btConeShape.h"
    17 #include "LinearMath/btPoint3.h"
    1817
    1918

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

    r2192 r2430  
    2323{
    2424        m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
    25         m_points.resize(numPoints);
     25        m_unscaledPoints.resize(numPoints);
    2626
    2727        unsigned char* pointsBaseAddress = (unsigned char*)points;
    … …  
    2929        for (int i=0;i<numPoints;i++)
    3030        {
    31                 btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride);
    32                 m_points[i] = point[0];
     31                btVector3* point = (btVector3*)(pointsBaseAddress + i*stride);
     32                m_unscaledPoints[i] = point[0];
    3333        }
    3434
    … …  
    4545}
    4646
    47 void btConvexHullShape::addPoint(const btPoint3& point)
     47void btConvexHullShape::addPoint(const btVector3& point)
    4848{
    49         m_points.push_back(point);
     49        m_unscaledPoints.push_back(point);
    5050        recalcLocalAabb();
    5151
    … …  
    6969
    7070
    71         for (int i=0;i<m_points.size();i++)
     71        for (int i=0;i<m_unscaledPoints.size();i++)
    7272        {
    73                 btPoint3 vtx = m_points[i] * m_localScaling;
     73                btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
    7474
    7575                newDot = vec.dot(vtx);
    … …  
    9393                }
    9494        }
    95         for (int i=0;i<m_points.size();i++)
     95        for (int i=0;i<m_unscaledPoints.size();i++)
    9696        {
    97                 btPoint3 vtx = m_points[i] * m_localScaling;
     97                btVector3 vtx = getScaledPoint(i);
    9898
    9999                for (int j=0;j<numVectors;j++)
    … …  
    146146int     btConvexHullShape::getNumVertices() const
    147147{
    148         return m_points.size();
     148        return m_unscaledPoints.size();
    149149}
    150150
    151151int btConvexHullShape::getNumEdges() const
    152152{
    153         return m_points.size();
     153        return m_unscaledPoints.size();
    154154}
    155155
    156 void btConvexHullShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const
     156void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const
    157157{
    158158
    159         int index0 = i%m_points.size();
    160         int index1 = (i+1)%m_points.size();
    161         pa = m_points[index0]*m_localScaling;
    162         pb = m_points[index1]*m_localScaling;
     159        int index0 = i%m_unscaledPoints.size();
     160        int index1 = (i+1)%m_unscaledPoints.size();
     161        pa = getScaledPoint(index0);
     162        pb = getScaledPoint(index1);
    163163}
    164164
    165 void btConvexHullShape::getVertex(int i,btPoint3& vtx) const
     165void btConvexHullShape::getVertex(int i,btVector3& vtx) const
    166166{
    167         vtx = m_points[i]*m_localScaling;
     167        vtx = getScaledPoint(i);
    168168}
    169169
    … …  
    173173}
    174174
    175 void btConvexHullShape::getPlane(btVector3& ,btPoint3& ,int ) const
     175void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
    176176{
    177177
    … …  
    180180
    181181//not yet
    182 bool btConvexHullShape::isInside(const btPoint3& ,btScalar ) const
     182bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
    183183{
    184184        assert(0);

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

    r2192 r2430  
    2525ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape
    2626{
    27         btAlignedObjectArray<btPoint3>  m_points;
     27        btAlignedObjectArray<btVector3> m_unscaledPoints;
    2828
    2929public:
    … …  
    3434        ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint.
    3535        ///btConvexHullShape make an internal copy of the points.
    36         btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btPoint3));
     36        btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3));
    3737
    38         void addPoint(const btPoint3& point);
     38        void addPoint(const btVector3& point);
    3939
    40         btPoint3* getPoints()
     40       
     41        btVector3* getUnscaledPoints()
    4142        {
    42                 return &m_points[0];
     43                return &m_unscaledPoints[0];
    4344        }
    4445
    45         const btPoint3* getPoints() const
     46        const btVector3* getUnscaledPoints() const
    4647        {
    47                 return &m_points[0];
     48                return &m_unscaledPoints[0];
    4849        }
    4950
    50         int getNumPoints() const
     51        ///getPoints is obsolete, please use getUnscaledPoints
     52        const btVector3* getPoints() const
    5153        {
    52                 return m_points.size();
     54                return getUnscaledPoints();
     55        }
     56
     57       
     58
     59
     60        SIMD_FORCE_INLINE       btVector3 getScaledPoint(int i) const
     61        {
     62                return m_unscaledPoints[i] * m_localScaling;
     63        }
     64
     65        SIMD_FORCE_INLINE       int getNumPoints() const
     66        {
     67                return m_unscaledPoints.size();
    5368        }
    5469
    … …  
    6580        virtual int     getNumVertices() const;
    6681        virtual int getNumEdges() const;
    67         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
    68         virtual void getVertex(int i,btPoint3& vtx) const;
     82        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
     83        virtual void getVertex(int i,btVector3& vtx) const;
    6984        virtual int     getNumPlanes() const;
    70         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const;
    71         virtual bool isInside(const btPoint3& pt,btScalar tolerance) const;
     85        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
     86        virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
    7287
    7388        ///in case we receive negative scaling

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

    r2192 r2430  
    1818
    1919
     20
    2021btConvexInternalShape::btConvexInternalShape()
    21 : btConvexShape (), m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
     22: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
    2223m_collisionMargin(CONVEX_DISTANCE_MARGIN)
    2324{
    … …  
    4950                minAabb[i] = tmp[i]-margin;
    5051        }
    51 };
     52}
     53
    5254
    5355
    … …  
    7173
    7274#else
     75        btAssert(0);
    7376        return btVector3(0,0,0);
    7477#endif //__SPU__

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

    r2192 r2430  
    3131        }
    3232
    33 
    3433        virtual btVector3       localGetSupportingVertex(const btVector3& vec)const;
    35 #ifndef __SPU__
    36         virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0;
    37        
    38         //notice that the vectors should be unit length
    39         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
    40 #endif //#ifndef __SPU__
    4134
    4235        const btVector3& getImplicitShapeDimensions() const

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

    r2192 r2430  
    1818#include "LinearMath/btQuaternion.h"
    1919
    20 btConvexPointCloudShape::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 
    29 void btConvexPointCloudShape::setPoints (btVector3* points, int numPoints)
    30 {
    31         m_points = points;
    32         m_numPoints = numPoints;
    33 
    34         recalcLocalAabb();
    35 }
    36 
    37 
    3820void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling)
    3921{
    … …  
    4224}
    4325
     26#ifndef __SPU__
    4427btVector3       btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
    4528{
    … …  
    6144        for (int i=0;i<m_numPoints;i++)
    6245        {
    63                 btPoint3 vtx = m_points[i] * m_localScaling;
     46                btVector3 vtx = getScaledPoint(i);
    6447
    6548                newDot = vec.dot(vtx);
    … …  
    8568        for (int i=0;i<m_numPoints;i++)
    8669        {
    87                 btPoint3 vtx = m_points[i] * m_localScaling;
     70                btVector3 vtx = getScaledPoint(i);
    8871
    8972                for (int j=0;j<numVectors;j++)
    … …  
    125108
    126109
    127 
     110#endif
    128111
    129112
    … …  
    144127}
    145128
    146 void btConvexPointCloudShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const
     129void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const
    147130{
    148131        btAssert (0);
    149132}
    150133
    151 void btConvexPointCloudShape::getVertex(int i,btPoint3& vtx) const
     134void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const
    152135{
    153         vtx = m_points[i]*m_localScaling;
     136        vtx = m_unscaledPoints[i]*m_localScaling;
    154137}
    155138
    … …  
    159142}
    160143
    161 void btConvexPointCloudShape::getPlane(btVector3& ,btPoint3& ,int ) const
     144void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const
    162145{
    163146
    … …  
    166149
    167150//not yet
    168 bool btConvexPointCloudShape::isInside(const btPoint3& ,btScalar ) const
     151bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const
    169152{
    170153        assert(0);

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

    r2192 r2430  
    2424ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape
    2525{
    26         btVector3* m_points;
     26        btVector3* m_unscaledPoints;
    2727        int m_numPoints;
     28
    2829public:
    2930        BT_DECLARE_ALIGNED_ALLOCATOR();
    3031
    31         btConvexPointCloudShape(btVector3* points,int numPoints);
     32        btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
     33        {
     34                m_localScaling = localScaling;
     35                m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
     36                m_unscaledPoints = points;
     37                m_numPoints = numPoints;
    3238
    33         void setPoints (btVector3* points, int numPoints);
    34 
    35         btPoint3* getPoints()
    36         {
    37                 return m_points;
     39                if (computeAabb)
     40                        recalcLocalAabb();
    3841        }
    3942
    40         const btPoint3* getPoints() const
     43        void setPoints (btVector3* points, int numPoints, bool computeAabb = true)
    4144        {
    42                 return m_points;
     45                m_unscaledPoints = points;
     46                m_numPoints = numPoints;
     47
     48                if (computeAabb)
     49                        recalcLocalAabb();
    4350        }
    4451
    45         int getNumPoints() const
     52        SIMD_FORCE_INLINE       btVector3* getUnscaledPoints()
     53        {
     54                return m_unscaledPoints;
     55        }
     56
     57        SIMD_FORCE_INLINE       const btVector3* getUnscaledPoints() const
     58        {
     59                return m_unscaledPoints;
     60        }
     61
     62        SIMD_FORCE_INLINE       int getNumPoints() const
    4663        {
    4764                return m_numPoints;
    4865        }
    4966
     67        SIMD_FORCE_INLINE       btVector3       getScaledPoint( int index) const
     68        {
     69                return m_unscaledPoints[index] * m_localScaling;
     70        }
     71
     72#ifndef __SPU__
    5073        virtual btVector3       localGetSupportingVertex(const btVector3& vec)const;
    5174        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
    5275        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
     76#endif
    5377
    5478
    … …  
    5882        virtual int     getNumVertices() const;
    5983        virtual int getNumEdges() const;
    60         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
    61         virtual void getVertex(int i,btPoint3& vtx) const;
     84        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
     85        virtual void getVertex(int i,btVector3& vtx) const;
    6286        virtual int     getNumPlanes() const;
    63         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const;
    64         virtual bool isInside(const btPoint3& pt,btScalar tolerance) const;
     87        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
     88        virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
    6589
    6690        ///in case we receive negative scaling

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

    r2192 r2430  
    1515
    1616#include "btConvexShape.h"
    17 #include "btConvexInternalShape.h"
    1817#include "btTriangleShape.h"
    1918#include "btSphereShape.h"
    … …  
    2322#include "btConvexPointCloudShape.h"
    2423
    25 static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints)
     24btConvexShape::btConvexShape ()
     25{
     26}
     27
     28btConvexShape::~btConvexShape()
     29{
     30
     31}
     32
     33
     34
     35static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling)
    2636{
    2737        btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
    … …  
    4252        for (int i=0;i<numPoints;i++)
    4353        {
    44                 btPoint3 vtx = points[i];// * m_localScaling;
     54                btVector3 vtx = points[i] * localScaling;
    4555
    4656                newDot = vec.dot(vtx);
    … …  
    6272                return btVector3(0,0,0);
    6373    }
    64         break;
    6574        case BOX_SHAPE_PROXYTYPE:
    6675        {
    67                 btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
     76                btBoxShape* convexShape = (btBoxShape*)this;
    6877                const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
    6978
    … …  
    7281                        btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
    7382        }
    74         break;
    7583        case TRIANGLE_SHAPE_PROXYTYPE:
    7684        {
    … …  
    8290                return btVector3(sup.getX(),sup.getY(),sup.getZ());
    8391        }
    84         break;
    8592        case CYLINDER_SHAPE_PROXYTYPE:
    8693        {
    … …  
    143150                }
    144151        }
    145         break;
    146152        case CAPSULE_SHAPE_PROXYTYPE:
    147153        {
    … …  
    200206                return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());   
    201207        }
    202         break;
    203208        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
    204209        {
    205210                btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
    206                 btVector3* points = convexPointCloudShape->getPoints ();
     211                btVector3* points = convexPointCloudShape->getUnscaledPoints ();
    207212                int numPoints = convexPointCloudShape->getNumPoints ();
    208                 return convexHullSupport (localDir, points, numPoints);
     213                return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
    209214        }
    210215        case CONVEX_HULL_SHAPE_PROXYTYPE:
    211216        {
    212217                btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
    213                 btPoint3* points = convexHullShape->getPoints ();
     218                btVector3* points = convexHullShape->getUnscaledPoints();
    214219                int numPoints = convexHullShape->getNumPoints ();
    215                 return convexHullSupport (localDir, points, numPoints);
    216         }
    217         break;
     220                return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
     221        }
    218222    default:
    219223#ifndef __SPU__
    … …  
    222226                btAssert (0);
    223227#endif
    224         break;
    225228        }
    226229
    227230        // should never reach here
    228231        btAssert (0);
    229         return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
     232        return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
    230233}
    231234
    … …  
    238241        }
    239242        localDirNorm.normalize ();
    240         switch (m_shapeType)
    241         {
    242     case SPHERE_SHAPE_PROXYTYPE:
    243         {
    244                 return btVector3(0,0,0) + getMarginNonVirtual() * localDirNorm;
    245     }
    246         break;
    247         case BOX_SHAPE_PROXYTYPE:
    248         {
    249                 btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
    250                 const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
    251 
    252                 return btVector3(localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
    253                                                 localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
    254                                                 localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()) + getMarginNonVirtual() * localDirNorm;
    255         }
    256         break;
    257         case TRIANGLE_SHAPE_PROXYTYPE:
    258         {
    259                 btTriangleShape* triangleShape = (btTriangleShape*)this;
    260                 btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
    261                 btVector3* vertices = &triangleShape->m_vertices1[0];
    262                 btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
    263                 btVector3 sup = vertices[dots.maxAxis()];
    264                 return btVector3(sup.getX(),sup.getY(),sup.getZ()) + getMarginNonVirtual() * localDirNorm;
    265         }
    266         break;
    267         case CYLINDER_SHAPE_PROXYTYPE:
    268         {
    269                 btCylinderShape* cylShape = (btCylinderShape*)this;
    270                 //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
    271 
    272                 btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
    273                 btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
    274                 int cylinderUpAxis = cylShape->getUpAxis();
    275                 int XX(1),YY(0),ZZ(2);
    276 
    277                 switch (cylinderUpAxis)
    278                 {
    279                 case 0:
    280                 {
    281                         XX = 1;
    282                         YY = 0;
    283                         ZZ = 2;
    284                 }
    285                 break;
    286                 case 1:
    287                 {
    288                         XX = 0;
    289                         YY = 1;
    290                         ZZ = 2;
    291                 }
    292                 break;
    293                 case 2:
    294                 {
    295                         XX = 0;
    296                         YY = 2;
    297                         ZZ = 1;
    298                        
    299                 }
    300                 break;
    301                 default:
    302                         btAssert(0);
    303                 break;
    304                 };
    305 
    306                 btScalar radius = halfExtents[XX];
    307                 btScalar halfHeight = halfExtents[cylinderUpAxis];
    308 
    309                 btVector3 tmp;
    310                 btScalar d ;
    311 
    312                 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
    313                 if (s != btScalar(0.0))
    314                 {
    315                         d = radius / s; 
    316                         tmp[XX] = v[XX] * d;
    317                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
    318                         tmp[ZZ] = v[ZZ] * d;
    319                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm;
    320                 } else {
    321                         tmp[XX] = radius;
    322                         tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
    323                         tmp[ZZ] = btScalar(0.0);
    324                         return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm;
    325                 }
    326         }
    327         break;
    328         case CAPSULE_SHAPE_PROXYTYPE:
    329         {
    330                 btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
    331 
    332                 btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
    333                 btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
    334                 btScalar halfHeight = capsuleShape->getHalfHeight();
    335                 int capsuleUpAxis = capsuleShape->getUpAxis();
    336 
    337                 btScalar radius = capsuleShape->getRadius();
    338                 btVector3 supVec(0,0,0);
    339 
    340                 btScalar maxDot(btScalar(-1e30));
    341 
    342                 btVector3 vec = vec0;
    343                 btScalar lenSqr = vec.length2();
    344                 if (lenSqr < btScalar(0.0001))
    345                 {
    346                         vec.setValue(1,0,0);
    347                 } else
    348                 {
    349                         btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
    350                         vec *= rlen;
    351                 }
    352                 btVector3 vtx;
    353                 btScalar newDot;
    354                 {
    355                         btVector3 pos(0,0,0);
    356                         pos[capsuleUpAxis] = halfHeight;
    357 
    358                         vtx = pos +vec*(radius);
    359                         newDot = vec.dot(vtx);
    360                         if (newDot > maxDot)
    361                         {
    362                                 maxDot = newDot;
    363                                 supVec = vtx;
    364                         }
    365                 }
    366                 {
    367                         btVector3 pos(0,0,0);
    368                         pos[capsuleUpAxis] = -halfHeight;
    369 
    370                         vtx = pos +vec*(radius);
    371                         newDot = vec.dot(vtx);
    372                         if (newDot > maxDot)
    373                         {
    374                                 maxDot = newDot;
    375                                 supVec = vtx;
    376                         }
    377                 }
    378                 return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()) + getMarginNonVirtual() * localDirNorm;
    379         }
    380         break;
    381         case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
    382         {
    383                 btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
    384                 btVector3* points = convexPointCloudShape->getPoints ();
    385                 int numPoints = convexPointCloudShape->getNumPoints ();
    386                 return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
    387         }
    388         case CONVEX_HULL_SHAPE_PROXYTYPE:
    389         {
    390                 btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
    391                 btPoint3* points = convexHullShape->getPoints ();
    392                 int numPoints = convexHullShape->getNumPoints ();
    393                 return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
    394         }
    395         break;
    396     default:
    397 #ifndef __SPU__
    398                 return this->localGetSupportingVertex (localDir);
    399 #else
    400                 btAssert (0);
    401 #endif
    402         break;
    403         }
    404 
    405         // should never reach here
    406         btAssert (0);
    407         return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
     243
     244        return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
    408245}
    409246
    … …  
    418255                return sphereShape->getRadius ();
    419256        }
    420         break;
    421257        case BOX_SHAPE_PROXYTYPE:
    422258        {
    423                 btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
     259                btBoxShape* convexShape = (btBoxShape*)this;
    424260                return convexShape->getMarginNV ();
    425261        }
    426         break;
    427262        case TRIANGLE_SHAPE_PROXYTYPE:
    428263        {
    … …  
    430265                return triangleShape->getMarginNV ();
    431266        }
    432         break;
    433267        case CYLINDER_SHAPE_PROXYTYPE:
    434268        {
    … …  
    436270                return cylShape->getMarginNV();
    437271        }
    438         break;
    439272        case CAPSULE_SHAPE_PROXYTYPE:
    440273        {
    … …  
    442275                return capsuleShape->getMarginNV();
    443276        }
    444         break;
    445277        case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
    446278        /* fall through */
    … …  
    450282                return convexHullShape->getMarginNV();
    451283        }
    452         break;
    453284    default:
    454285#ifndef __SPU__
    … …  
    457288                btAssert (0);
    458289#endif
    459         break;
    460290        }
    461291
    … …  
    484314        case BOX_SHAPE_PROXYTYPE:
    485315        {
    486                 btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
     316                btBoxShape* convexShape = (btBoxShape*)this;
    487317                float margin=convexShape->getMarginNonVirtual();
    488318                btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
    489319                halfExtents += btVector3(margin,margin,margin);
    490320                btMatrix3x3 abs_b = t.getBasis().absolute(); 
    491                 btPoint3 center = t.getOrigin();
     321                btVector3 center = t.getOrigin();
    492322                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
    493323               
    … …  
    496326                break;
    497327        }
    498         break;
    499328        case TRIANGLE_SHAPE_PROXYTYPE:
    500329        {
    … …  
    524353                halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
    525354                btMatrix3x3 abs_b = t.getBasis().absolute(); 
    526                 btPoint3 center = t.getOrigin();
     355                btVector3 center = t.getOrigin();
    527356                btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));                   
    528357                aabbMin = center - extent;

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

    r2192 r2430  
    2525#include "LinearMath/btAlignedAllocator.h"
    2626
    27 //todo: get rid of this btConvexCastResult thing!
    28 struct btConvexCastResult;
    2927#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10
    3028
    … …  
    3937        BT_DECLARE_ALIGNED_ALLOCATOR();
    4038
    41         btConvexShape ()
    42         {
    43         }
     39        btConvexShape ();
    4440
    45         virtual ~btConvexShape()
    46         {
     41        virtual ~btConvexShape();
    4742
    48         }
     43        virtual btVector3       localGetSupportingVertex(const btVector3& vec)const = 0;
    4944
    50 
    51         virtual btVector3       localGetSupportingVertex(const btVector3& vec)const =0;
    52 #ifndef __SPU__
    53         virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0;
    54        
    55         //notice that the vectors should be unit length
    56         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
    57 #endif //#ifndef __SPU__
     45        ////////
     46        #ifndef __SPU__
     47        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0;
     48        #endif //#ifndef __SPU__
    5849
    5950        btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const;
    … …  
    6152        btScalar getMarginNonVirtual () const;
    6253        void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
     54
     55       
     56        //notice that the vectors should be unit length
     57        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
    6358
    6459        ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
    … …  
    7873        virtual void    getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0;
    7974
     75
     76       
     77       
    8078};
    8179

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

    r2192 r2430  
    109109        }
    110110       
    111         //todo: could do the batch inside the callback!
     111        ///@todo: could do the batch inside the callback!
    112112
    113113
    … …  
    164164}
    165165
    166 void btConvexTriangleMeshShape::getEdge(int ,btPoint3& ,btPoint3& ) const
     166void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
    167167{
    168168        btAssert(0);   
    169169}
    170170
    171 void btConvexTriangleMeshShape::getVertex(int ,btPoint3& ) const
     171void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
    172172{
    173173        btAssert(0);
    … …  
    179179}
    180180
    181 void btConvexTriangleMeshShape::getPlane(btVector3& ,btPoint3& ,int  ) const
     181void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int  ) const
    182182{
    183183        btAssert(0);
    … …  
    185185
    186186//not yet
    187 bool btConvexTriangleMeshShape::isInside(const btPoint3& ,btScalar ) const
     187bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
    188188{
    189189        btAssert(0);
    … …  
    270270         btVector3 b = triangle[1] - center;
    271271         btVector3 c = triangle[2] - center;
    272          btVector3 abc = a + b + c;
    273272         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
    274273         for (int j = 0; j < 3; j++)
    … …  
    276275            for (int k = 0; k <= j; k++)
    277276            {
    278                i[j][k] = i[k][j] = volNeg * (center[j] * center[k]
    279                  + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j])
    280                   + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
     277               i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
    281278                  + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
    282279            }

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

    r2192 r2430  
    3535        virtual int     getNumVertices() const;
    3636        virtual int getNumEdges() const;
    37         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
    38         virtual void getVertex(int i,btPoint3& vtx) const;
     37        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
     38        virtual void getVertex(int i,btVector3& vtx) const;
    3939        virtual int     getNumPlanes() const;
    40         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const;
    41         virtual bool isInside(const btPoint3& pt,btScalar tolerance) const;
     40        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
     41        virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
    4242
    4343       

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

    r2192 r2430  
    1414*/
    1515#include "btCylinderShape.h"
    16 #include "LinearMath/btPoint3.h"
    1716
    1817btCylinderShape::btCylinderShape (const btVector3& halfExtents)

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

    r2192 r2430  
    5757        }
    5858
     59        virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const
     60        {
     61        }
    5962
    6063protected:

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

    r2192 r2430  
    1919
    2020
     21
     22btHeightfieldTerrainShape::btHeightfieldTerrainShape
     23(
     24int heightStickWidth, int heightStickLength, void* heightfieldData,
     25btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis,
     26PHY_ScalarType hdt, bool flipQuadEdges
     27)
     28{
     29        initialize(heightStickWidth, heightStickLength, heightfieldData,
     30                   heightScale, minHeight, maxHeight, upAxis, hdt,
     31                   flipQuadEdges);
     32}
     33
     34
     35
    2136btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
    22 : btConcaveShape (), m_heightStickWidth(heightStickWidth),
    23 m_heightStickLength(heightStickLength),
    24 m_maxHeight(maxHeight),
    25 m_width((btScalar)heightStickWidth-1),
    26 m_length((btScalar)heightStickLength-1),
    27 m_heightfieldDataUnknown(heightfieldData),
    28 m_useFloatData(useFloatData),
    29 m_flipQuadEdges(flipQuadEdges),
    30 m_useDiamondSubdivision(false),
    31 m_upAxis(upAxis),
    32 m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
    33 {
     37{
     38        // legacy constructor: support only float or unsigned char,
     39        //      and min height is zero
     40        PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
     41        btScalar minHeight = 0.0;
     42
     43        // previously, height = uchar * maxHeight / 65535.
     44        // So to preserve legacy behavior, heightScale = maxHeight / 65535
     45        btScalar heightScale = maxHeight / 65535;
     46
     47        initialize(heightStickWidth, heightStickLength, heightfieldData,
     48                   heightScale, minHeight, maxHeight, upAxis, hdt,
     49                   flipQuadEdges);
     50}
     51
     52
     53
     54void btHeightfieldTerrainShape::initialize
     55(
     56int heightStickWidth, int heightStickLength, void* heightfieldData,
     57btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
     58PHY_ScalarType hdt, bool flipQuadEdges
     59)
     60{
     61        // validation
     62        btAssert(heightStickWidth > 1 && "bad width");
     63        btAssert(heightStickLength > 1 && "bad length");
     64        btAssert(heightfieldData && "null heightfield data");
     65        // btAssert(heightScale) -- do we care?  Trust caller here
     66        btAssert(minHeight <= maxHeight && "bad min/max height");
     67        btAssert(upAxis >= 0 && upAxis < 3 &&
     68            "bad upAxis--should be in range [0,2]");
     69        btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
     70            "Bad height data type enum");
     71
     72        // initialize member variables
    3473        m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
    35 
    36         btScalar        quantizationMargin = 1.f;
    37 
    38         //enlarge the AABB to avoid division by zero when initializing the quantization values
    39         btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
    40 
    41         btVector3       halfExtents(0,0,0);
    42 
     74        m_heightStickWidth = heightStickWidth;
     75        m_heightStickLength = heightStickLength;
     76        m_minHeight = minHeight;
     77        m_maxHeight = maxHeight;
     78        m_width = (btScalar) (heightStickWidth - 1);
     79        m_length = (btScalar) (heightStickLength - 1);
     80        m_heightScale = heightScale;
     81        m_heightfieldDataUnknown = heightfieldData;
     82        m_heightDataType = hdt;
     83        m_flipQuadEdges = flipQuadEdges;
     84        m_useDiamondSubdivision = false;
     85        m_upAxis = upAxis;
     86        m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
     87
     88        // determine min/max axis-aligned bounding box (aabb) values
    4389        switch (m_upAxis)
    4490        {
    4591        case 0:
    4692                {
    47                         halfExtents.setValue(
    48                                 btScalar(m_maxHeight),
    49                                 btScalar(m_width), //?? don't know if this should change
    50                                 btScalar(m_length));
     93                        m_localAabbMin.setValue(m_minHeight, 0, 0);
     94                        m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
    5195                        break;
    5296                }
    5397        case 1:
    5498                {
    55                         halfExtents.setValue(
    56                                 btScalar(m_width),
    57                                 btScalar(m_maxHeight),
    58                                 btScalar(m_length));
     99                        m_localAabbMin.setValue(0, m_minHeight, 0);
     100                        m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
    59101                        break;
    60102                };
    61103        case 2:
    62104                {
    63                         halfExtents.setValue(
    64                                 btScalar(m_width),
    65                                 btScalar(m_length),
    66                                 btScalar(m_maxHeight)
    67                         );
     105                        m_localAabbMin.setValue(0, 0, m_minHeight);
     106                        m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
    68107                        break;
    69108                }
    … …  
    71110                {
    72111                        //need to get valid m_upAxis
    73                         btAssert(0);
    74                 }
    75         }
    76 
    77         halfExtents*= btScalar(0.5);
    78        
    79         m_localAabbMin = -halfExtents - clampValue;
    80         m_localAabbMax = halfExtents + clampValue;
    81         btVector3 aabbSize = m_localAabbMax - m_localAabbMin;
    82 
    83 }
     112                        btAssert(0 && "Bad m_upAxis");
     113                }
     114        }
     115
     116        // remember origin (defined as exact middle of aabb)
     117        m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
     118}
     119
    84120
    85121
    … …  
    93129{
    94130        btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
    95         halfExtents += btVector3(getMargin(),getMargin(),getMargin());
     131
     132        btVector3 localOrigin(0, 0, 0);
     133        localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
     134        localOrigin *= m_localScaling;
    96135
    97136        btMatrix3x3 abs_b = t.getBasis().absolute(); 
    98         btPoint3 center = t.getOrigin();
     137        btVector3 center = t.getOrigin();
    99138        btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
    100139                   abs_b[1].dot(halfExtents),
    101140                  abs_b[2].dot(halfExtents));
    102        
     141        extent += btVector3(getMargin(),getMargin(),getMargin());
    103142
    104143        aabbMin = center - extent;
    105144        aabbMax = center + extent;
    106 
    107 
    108145}
    109146
    … …  
    111148{
    112149        btScalar val = 0.f;
    113         if (m_useFloatData)
     150        switch (m_heightDataType)
    114151        {
    115                 val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
    116         } else
    117         {
    118                 //assume unsigned short int
    119                 unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
    120                 val = heightFieldValue* (m_maxHeight/btScalar(65535));
    121         }
     152        case PHY_FLOAT:
     153                {
     154                        val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
     155                        break;
     156                }
     157
     158        case PHY_UCHAR:
     159                {
     160                        unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
     161                        val = heightFieldValue * m_heightScale;
     162                        break;
     163                }
     164
     165        case PHY_SHORT:
     166                {
     167                        short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x];
     168                        val = hfValue * m_heightScale;
     169                        break;
     170                }
     171
     172        default:
     173                {
     174                        btAssert(!"Bad m_heightDataType");
     175                }
     176        }
     177
    122178        return val;
    123179}
    … …  
    179235
    180236
     237
     238static inline int
     239getQuantized
     240(
     241float x
     242)
     243{
     244        if (x < 0.0) {
     245                return (int) (x - 0.5);
     246        }
     247        return (int) (x + 0.5);
     248}
     249
     250
     251
     252/// given input vector, return quantized version
     253/**
     254  This routine is basically determining the gridpoint indices for a given
     255  input vector, answering the question: "which gridpoint is closest to the
     256  provided point?".
     257
     258  "with clamp" means that we restrict the point to be in the heightfield's
     259  axis-aligned bounding box.
     260 */
    181261void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
    182262{
    … …  
    185265        clampedPoint.setMin(m_localAabbMax);
    186266
    187         btVector3 v = (clampedPoint);// - m_bvhAabbMin) * m_bvhQuantization;
    188 
    189         //TODO: optimization: check out how to removed this btFabs
     267        out[0] = getQuantized(clampedPoint.getX());
     268        out[1] = getQuantized(clampedPoint.getY());
     269        out[2] = getQuantized(clampedPoint.getZ());
    190270               
    191         out[0] = (int)(v.getX() + v.getX() / btFabs(v.getX())* btScalar(0.5) );
    192         out[1] = (int)(v.getY() + v.getY() / btFabs(v.getY())* btScalar(0.5) );
    193         out[2] = (int)(v.getZ() + v.getZ() / btFabs(v.getZ())* btScalar(0.5) );
    194        
    195 }
    196 
    197 
     271}
     272
     273
     274
     275/// process all triangles within the provided axis-aligned bounding box
     276/**
     277  basic algorithm:
     278    - convert input aabb to local coordinates (scale down and shift for local origin)
     279    - convert input aabb to a range of heightfield grid points (quantize)
     280    - iterate over all triangles in that subset of the grid
     281 */
    198282void    btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
    199283{
    200         (void)callback;
    201         (void)aabbMax;
    202         (void)aabbMin;
     284        // scale down the input aabb's so they are in local (non-scaled) coordinates
     285        btVector3       localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
     286        btVector3       localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
     287
     288        // account for local origin
     289        localAabbMin += m_localOrigin;
     290        localAabbMax += m_localOrigin;
    203291
    204292        //quantize the aabbMin and aabbMax, and adjust the start/end ranges
    205 
    206293        int     quantizedAabbMin[3];
    207294        int     quantizedAabbMax[3];
    208 
    209         btVector3       localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
    210         btVector3       localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
    211        
    212295        quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
    213296        quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
    214297       
    215        
     298        // expand the min/max quantized values
     299        // this is to catch the case where the input aabb falls between grid points!
     300        for (int i = 0; i < 3; ++i) {
     301                quantizedAabbMin[i]--;
     302                quantizedAabbMax[i]++;
     303        }       
    216304
    217305        int startX=0;
    … …  
    224312        case 0:
    225313                {
    226                         quantizedAabbMin[1]+=m_heightStickWidth/2-1;
    227                         quantizedAabbMax[1]+=m_heightStickWidth/2+1;
    228                         quantizedAabbMin[2]+=m_heightStickLength/2-1;
    229                         quantizedAabbMax[2]+=m_heightStickLength/2+1;
    230 
    231314                        if (quantizedAabbMin[1]>startX)
    232315                                startX = quantizedAabbMin[1];
    … …  
    241324        case 1:
    242325                {
    243                         quantizedAabbMin[0]+=m_heightStickWidth/2-1;
    244                         quantizedAabbMax[0]+=m_heightStickWidth/2+1;
    245                         quantizedAabbMin[2]+=m_heightStickLength/2-1;
    246                         quantizedAabbMax[2]+=m_heightStickLength/2+1;
    247 
    248326                        if (quantizedAabbMin[0]>startX)
    249327                                startX = quantizedAabbMin[0];
    … …  
    258336        case 2:
    259337                {
    260                         quantizedAabbMin[0]+=m_heightStickWidth/2-1;
    261                         quantizedAabbMax[0]+=m_heightStickWidth/2+1;
    262                         quantizedAabbMin[1]+=m_heightStickLength/2-1;
    263                         quantizedAabbMax[1]+=m_heightStickLength/2+1;
    264 
    265338                        if (quantizedAabbMin[0]>startX)
    266339                                startX = quantizedAabbMin[0];

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

    r2192 r2430  
    1919#include "btConcaveShape.h"
    2020
    21 ///The btHeightfieldTerrainShape simulates a 2D heightfield terrain collision shape. You can also use the more general btBvhTriangleMeshShape instead.
    22 ///An example implementation of btHeightfieldTerrainShape is provided in Demos/VehicleDemo/VehicleDemo.cpp
     21///btHeightfieldTerrainShape simulates a 2D heightfield terrain
     22/**
     23  The caller is responsible for maintaining the heightfield array; this
     24  class does not make a copy.
     25
     26  The heightfield can be dynamic so long as the min/max height values
     27  capture the extremes (heights must always be in that range).
     28
     29  The local origin of the heightfield is assumed to be the exact
     30  center (as determined by width and length and height, with each
     31  axis multiplied by the localScaling).
     32
     33  Most (but not all) rendering and heightfield libraries assume upAxis = 1
     34  (that is, the y-axis is "up").  This class allows any of the 3 coordinates
     35  to be "up".  Make sure your choice of axis is consistent with your rendering
     36  system.
     37
     38  The heightfield heights are determined from the data type used for the
     39  heightfieldData array. 
     40
     41   - PHY_UCHAR: height at a point is the uchar value at the
     42       grid point, multipled by heightScale.  uchar isn't recommended
     43       because of its inability to deal with negative values, and
     44       low resolution (8-bit).
     45
     46   - PHY_SHORT: height at a point is the short int value at that grid
     47       point, multipled by heightScale.
     48
     49   - PHY_FLOAT: height at a point is the float value at that grid
     50       point.  heightScale is ignored when using the float heightfield
     51       data type.
     52
     53  Whatever the caller specifies as minHeight and maxHeight will be honored.
     54  The class will not inspect the heightfield to discover the actual minimum
     55  or maximum heights.  These values are used to determine the heightfield's
     56  axis-aligned bounding box, multiplied by localScaling.
     57
     58  For usage and testing see the TerrainDemo.
     59 */
    2360class btHeightfieldTerrainShape : public btConcaveShape
    2461{
    … …  
    2663        btVector3       m_localAabbMin;
    2764        btVector3       m_localAabbMax;
    28        
     65        btVector3       m_localOrigin;
     66
    2967        ///terrain data
    3068        int     m_heightStickWidth;
    3169        int m_heightStickLength;
     70        btScalar        m_minHeight;
    3271        btScalar        m_maxHeight;
    3372        btScalar m_width;
    3473        btScalar m_length;
     74        btScalar m_heightScale;
    3575        union
    3676        {
    3777                unsigned char*  m_heightfieldDataUnsignedChar;
     78                short*          m_heightfieldDataShort;
    3879                btScalar*                       m_heightfieldDataFloat;
    3980                void*                   m_heightfieldDataUnknown;
    4081        };
    41        
    42         bool    m_useFloatData;
     82
     83        PHY_ScalarType  m_heightDataType;       
    4384        bool    m_flipQuadEdges;
    4485  bool  m_useDiamondSubdivision;
    … …  
    5293        void            getVertex(int x,int y,btVector3& vertex) const;
    5394
    54         inline bool testQuantizedAabbAgainstQuantizedAabb(int* aabbMin1, int* aabbMax1,const  int* aabbMin2,const  int* aabbMax2) const
    55         {
    56                 bool overlap = true;
    57                 overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
    58                 overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
    59                 overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
    60                 return overlap;
    61         }
     95
     96
     97        /// protected initialization
     98        /**
     99          Handles the work of constructors so that public constructors can be
     100          backwards-compatible without a lot of copy/paste.
     101         */
     102        void initialize(int heightStickWidth, int heightStickLength,
     103                        void* heightfieldData, btScalar heightScale,
     104                        btScalar minHeight, btScalar maxHeight, int upAxis,
     105                        PHY_ScalarType heightDataType, bool flipQuadEdges);
    62106
    63107public:
    64         btHeightfieldTerrainShape(int heightStickWidth,int heightStickHeight,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges);
     108        /// preferred constructor
     109        /**
     110          This constructor supports a range of heightfield
     111          data types, and allows for a non-zero minimum height value.
     112          heightScale is needed for any integer-based heightfield data types.
     113         */
     114        btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,
     115                                  void* heightfieldData, btScalar heightScale,
     116                                  btScalar minHeight, btScalar maxHeight,
     117                                  int upAxis, PHY_ScalarType heightDataType,
     118                                  bool flipQuadEdges);
     119
     120        /// legacy constructor
     121        /**
     122          The legacy constructor assumes the heightfield has a minimum height
     123          of zero.  Only unsigned char or floats are supported.  For legacy
     124          compatibility reasons, heightScale is calculated as maxHeight / 65535
     125          (and is only used when useFloatData = false).
     126         */
     127        btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges);
    65128
    66129        virtual ~btHeightfieldTerrainShape();

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

    r2192 r2430  
    3636void    btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
    3737{
    38         //todo: could make recursive use of batching. probably this shape is not used frequently.
     38        ///@todo: could make recursive use of batching. probably this shape is not used frequently.
    3939        for (int i=0;i<numVectors;i++)
    4040        {

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

    r2192 r2430  
    320320                                       
    321321                                        int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
    322                                         btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
    323 #ifdef DEBUG_PATCH_COLORS
    324                                         btVector3 mycolor = color[index&3];
    325                                         graphicsbase[8] = mycolor.getX();
    326                                         graphicsbase[9] = mycolor.getY();
    327                                         graphicsbase[10] = mycolor.getZ();
    328 #endif //DEBUG_PATCH_COLORS
    329 
    330 
    331                                         triangleVerts[j] = btVector3(
    332                                                 graphicsbase[0]*meshScaling.getX(),
    333                                                 graphicsbase[1]*meshScaling.getY(),
    334                                                 graphicsbase[2]*meshScaling.getZ());
     322                                        if (type == PHY_FLOAT)
     323                                        {
     324                                                float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
     325                                                triangleVerts[j] = btVector3(
     326                                                        graphicsbase[0]*meshScaling.getX(),
     327                                                        graphicsbase[1]*meshScaling.getY(),
     328                                                        graphicsbase[2]*meshScaling.getZ());
     329                                        }
     330                                        else
     331                                        {
     332                                                double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
     333                                                triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
     334                                        }
    335335                                }
    336336

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

    r2192 r2430  
    1616#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
    1717
    18 btPolyhedralConvexShape::btPolyhedralConvexShape()
    19 :btConvexInternalShape(),
     18btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
    2019m_localAabbMin(1,1,1),
    2120m_localAabbMax(-1,-1,-1),
    … …  
    2524
    2625}
    27 
    2826
    2927

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

    r2192 r2430  
    1717#define BU_SHAPE
    1818
    19 #include "LinearMath/btPoint3.h"
    2019#include "LinearMath/btMatrix3x3.h"
    2120#include "LinearMath/btAabbUtil2.h"
    … …  
    3231        bool            m_isLocalAabbValid;
    3332
    34         btPolyhedralConvexShape();
    3533public:
    3634
    37        
     35        btPolyhedralConvexShape();
    3836
    3937        //brute force implementations
     38
    4039        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
    4140        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
     41
    4242       
    4343        virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
    4444
     45
     46        void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
     47        {
     48                m_isLocalAabbValid = true;
     49                m_localAabbMin = aabbMin;
     50                m_localAabbMax = aabbMax;
     51        }
     52
     53        inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
     54        {
     55                btAssert(m_isLocalAabbValid);
     56                aabbMin = m_localAabbMin;
     57                aabbMax = m_localAabbMax;
     58        }
    4559
    4660        inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
    … …  
    6175        virtual int     getNumVertices() const = 0 ;
    6276        virtual int getNumEdges() const = 0;
    63         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const = 0;
    64         virtual void getVertex(int i,btPoint3& vtx) const = 0;
     77        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0;
     78        virtual void getVertex(int i,btVector3& vtx) const = 0;
    6579        virtual int     getNumPlanes() const = 0;
    66         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const = 0;
     80        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0;
    6781//      virtual int getIndex(int i) const = 0 ;
    6882
    69         virtual bool isInside(const btPoint3& pt,btScalar tolerance) const = 0;
     83        virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0;
    7084       
    7185        /// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp

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

    r2192 r2430  
    1616#include "btScaledBvhTriangleMeshShape.h"
    1717
    18 btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling)
     18btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling)
    1919:m_localScaling(localScaling),m_bvhTriMeshShape(childShape)
    2020{
    … …  
    3535public:
    3636
    37         btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling)
     37        btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling)
    3838                :m_originalCallback(originalCallback),
    3939                m_localScaling(localScaling)
    … …  
    9494        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
    9595
    96         btPoint3 center = trans(localCenter);
     96        btVector3 center = trans(localCenter);
    9797
    9898        btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),

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

    r2192 r2430  
    3333
    3434
    35         btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling);
     35        btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling);
    3636
    3737        virtual ~btScaledBvhTriangleMeshShape();

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

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

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

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

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

    r2192 r2430  
    1919#include "LinearMath/btVector3.h"
    2020#include "btTriangleCallback.h"
     21#include "btConcaveShape.h"
    2122
    22 /// PHY_ScalarType enumerates possible scalar types.
    23 /// See the btStridingMeshInterface for its use
    24 typedef enum PHY_ScalarType {
    25         PHY_FLOAT,
    26         PHY_DOUBLE,
    27         PHY_INTEGER,
    28         PHY_SHORT,
    29         PHY_FIXEDPOINT88
    30 } PHY_ScalarType;
     23
    3124
    3225///     The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes.
    … …  
    7871
    7972                virtual bool    hasPremadeAabb() const { return false; }
    80                 virtual void    setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const {}
    81                 virtual void    getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const {}
     73                virtual void    setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
     74                {
     75                        (void) aabbMin;
     76                        (void) aabbMax;
     77                }
     78                virtual void    getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const
     79        {
     80            (void) aabbMin;
     81            (void) aabbMax;
     82        }
    8283
    8384                const btVector3&        getScaling() const {

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

    r2192 r2430  
    2323}
    2424
    25 btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0) : btPolyhedralConvexShape (),
    26 m_numVertices(0)
    27 {
    28         m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
    29         addVertex(pt0);
    30 }
    31 
    32 btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1) : btPolyhedralConvexShape (),
     25btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexShape (),
     26m_numVertices(0)
     27{
     28        m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
     29        addVertex(pt0);
     30}
     31
     32btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexShape (),
    3333m_numVertices(0)
    3434{
    … …  
    3838}
    3939
    40 btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2) : btPolyhedralConvexShape (),
     40btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexShape (),
    4141m_numVertices(0)
    4242{
    … …  
    4747}
    4848
    49 btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3) : btPolyhedralConvexShape (),
     49btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexShape (),
    5050m_numVertices(0)
    5151{
    … …  
    6161
    6262
    63 void btBU_Simplex1to4::addVertex(const btPoint3& pt)
     63void btBU_Simplex1to4::addVertex(const btVector3& pt)
    6464{
    6565        m_vertices[m_numVertices++] = pt;
    … …  
    9393}
    9494
    95 void btBU_Simplex1to4::getEdge(int i,btPoint3& pa,btPoint3& pb) const
     95void btBU_Simplex1to4::getEdge(int i,btVector3& pa,btVector3& pb) const
    9696{
    9797       
    … …  
    157157}
    158158
    159 void btBU_Simplex1to4::getVertex(int i,btPoint3& vtx) const
     159void btBU_Simplex1to4::getVertex(int i,btVector3& vtx) const
    160160{
    161161        vtx = m_vertices[i];
    … …  
    184184
    185185
    186 void btBU_Simplex1to4::getPlane(btVector3&, btPoint3& ,int ) const
     186void btBU_Simplex1to4::getPlane(btVector3&, btVector3& ,int ) const
    187187{
    188188       
    … …  
    194194}
    195195
    196 bool btBU_Simplex1to4::isInside(const btPoint3& ,btScalar ) const
     196bool btBU_Simplex1to4::isInside(const btVector3& ,btScalar ) const
    197197{
    198198        return false;

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

    r2192 r2430  
    2828
    2929        int     m_numVertices;
    30         btPoint3        m_vertices[4];
     30        btVector3       m_vertices[4];
    3131
    3232public:
    3333        btBU_Simplex1to4();
    3434
    35         btBU_Simplex1to4(const btPoint3& pt0);
    36         btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1);
    37         btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2);
    38         btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3);
     35        btBU_Simplex1to4(const btVector3& pt0);
     36        btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1);
     37        btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2);
     38        btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3);
    3939
    4040   
    … …  
    4646
    4747
    48         void addVertex(const btPoint3& pt);
     48        void addVertex(const btVector3& pt);
    4949
    5050        //PolyhedralConvexShape interface
    … …  
    5454        virtual int getNumEdges() const;
    5555
    56         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
     56        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
    5757       
    58         virtual void getVertex(int i,btPoint3& vtx) const;
     58        virtual void getVertex(int i,btVector3& vtx) const;
    5959
    6060        virtual int     getNumPlanes() const;
    6161
    62         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const;
     62        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const;
    6363
    6464        virtual int getIndex(int i) const;
    6565
    66         virtual bool isInside(const btPoint3& pt,btScalar tolerance) const;
     66        virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
    6767
    6868

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

    r2192 r2430  
    2929};
    3030
    31 ///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
     31///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
    3232///Example usage of this class:
    3333///                     btTriangleBuffer        triBuf;

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

    r2192 r2430  
    7575}
    7676
    77 int     btTriangleMesh::findOrAddVertex(const btVector3& vertex)
     77
     78int     btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
    7879{
    7980        //return index of new/existing vertex
    80         //todo: could use acceleration structure for this
     81        ///@todo: could use acceleration structure for this
    8182        if (m_use4componentVertices)
    8283        {
    83                 for (int i=0;i< m_4componentVertices.size();i++)
    84                 {
    85                         if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold)
     84                if (removeDuplicateVertices)
    8685                        {
    87                                 return i;
     86                        for (int i=0;i< m_4componentVertices.size();i++)
     87                        {
     88                                if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold)
     89                                {
     90                                        return i;
     91                                }
    8892                        }
    8993                }
    … …  
    97101        {
    98102               
    99                 for (int i=0;i< m_3componentVertices.size();i+=3)
     103                if (removeDuplicateVertices)
    100104                {
    101                         btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]);
    102                         if ((vtx-vertex).length2() <= m_weldingThreshold)
     105                        for (int i=0;i< m_3componentVertices.size();i+=3)
    103106                        {
    104                                 return i/3;
     107                                btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]);
     108                                if ((vtx-vertex).length2() <= m_weldingThreshold)
     109                                {
     110                                        return i/3;
     111                                }
    105112                        }
    106                 }
     113        }
    107114                m_3componentVertices.push_back(vertex.getX());
    108115                m_3componentVertices.push_back(vertex.getY());
    … …  
    115122}
    116123               
    117 void    btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2)
     124void    btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices)
    118125{
    119126        m_indexedMeshes[0].m_numTriangles++;
    120                
    121         addIndex(findOrAddVertex(vertex0));
    122         addIndex(findOrAddVertex(vertex1));
    123         addIndex(findOrAddVertex(vertex2));
     127        addIndex(findOrAddVertex(vertex0,removeDuplicateVertices));
     128        addIndex(findOrAddVertex(vertex1,removeDuplicateVertices));
     129        addIndex(findOrAddVertex(vertex2,removeDuplicateVertices));
    124130}
    125131

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

    r2192 r2430  
    2626///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface.
    2727///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same.
    28 ///It has a brute-force option to weld together closeby vertices.
    2928class btTriangleMesh : public btTriangleIndexVertexArray
    3029{
    … …  
    4342                btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true);
    4443
    45                 int             findOrAddVertex(const btVector3& vertex);
     44                int             findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
    4645                void    addIndex(int index);
    4746
    … …  
    5554                        return m_use4componentVertices;
    5655                }
    57                
    58                 void    addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2);
     56                ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes.
     57                ///In general it is better to directly use btTriangleIndexVertexArray instead.
     58                void    addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false);
    5959               
    6060                int getNumTriangles() const;

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

    r2192 r2430  
    5454        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
    5555
    56         btPoint3 center = trans(localCenter);
     56        btVector3 center = trans(localCenter);
    5757
    5858        btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),

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

    r2192 r2430  
    4747        }
    4848       
    49         virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const
     49        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
    5050        {
    5151                getVertex(i,pa);
    … …  
    8989
    9090
    91         virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const
     91        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const
    9292        {
    9393                getPlaneEquation(i,planeNormal,planeSupport);
    … …  
    105105        }
    106106
    107         virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const
     107        virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const
    108108        {
    109109                (void)i;
    … …  
    119119        }
    120120
    121                 virtual bool isInside(const btPoint3& pt,btScalar tolerance) const
     121                virtual bool isInside(const btVector3& pt,btScalar tolerance) const
    122122        {
    123123                btVector3 normal;
    … …  
    133133                        for (i=0;i<3;i++)
    134134                        {
    135                                 btPoint3 pa,pb;
     135                                btVector3 pa,pb;
    136136                                getEdge(i,pa,pb);
    137137                                btVector3 edge = pb-pa;

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h

    r2192 r2430  
    2222#include "btSimplexSolverInterface.h"
    2323class btConvexShape;
    24 #include "LinearMath/btPoint3.h"
    2524class btTransform;
    2625
    … …  
    3433                const btConvexShape* convexA,const btConvexShape* convexB,
    3534                                        const btTransform& transA,const btTransform& transB,
    36                                 btVector3& v, btPoint3& pa, btPoint3& pb,
     35                                btVector3& v, btVector3& pa, btVector3& pb,
    3736                                class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
    3837                                ) = 0;

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

    r2192 r2430  
    3838{
    3939
    40 // Config
     40        // Config
    4141
    4242        /* GJK  */
    … …  
    5959
    6060
    61 // Shorthands
    62 typedef unsigned int    U;
    63 typedef unsigned char   U1;
    64 
    65 // MinkowskiDiff
    66 struct  MinkowskiDiff
     61        // Shorthands
     62        typedef unsigned int    U;
     63        typedef unsigned char   U1;
     64
     65        // MinkowskiDiff
     66        struct  MinkowskiDiff
    6767        {
    68         const btConvexShape*    m_shapes[2];
    69         btMatrix3x3                             m_toshape1;
    70         btTransform                             m_toshape0;
    71         btVector3                               (btConvexShape::*Ls)(const btVector3&) const;
    72         void                                    EnableMargin(bool enable)
    73                 {
    74                 if(enable)
    75                         Ls=&btConvexShape::localGetSupportVertexNonVirtual;
     68                const btConvexShape*    m_shapes[2];
     69                btMatrix3x3                             m_toshape1;
     70                btTransform                             m_toshape0;
     71                btVector3                               (btConvexShape::*Ls)(const btVector3&) const;
     72                void                                    EnableMargin(bool enable)
     73                {
     74                        if(enable)
     75                                Ls=&btConvexShape::localGetSupportVertexNonVirtual;
    7676                        else
    77                         Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
     77                                Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
    7878                }       
    79         inline btVector3                Support0(const btVector3& d) const
    80                 {
    81                 return(((m_shapes[0])->*(Ls))(d));
     79                inline btVector3                Support0(const btVector3& d) const
     80                {
     81                        return(((m_shapes[0])->*(Ls))(d));
    8282                }
    83         inline btVector3                Support1(const btVector3& d) const
    84                 {
    85                 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
     83                inline btVector3                Support1(const btVector3& d) const
     84                {
     85                        return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
    8686                }
    87         inline btVector3                Support(const btVector3& d) const
    88                 {
    89                 return(Support0(d)-Support1(-d));
     87                inline btVector3                Support(const btVector3& d) const
     88                {
     89                        return(Support0(d)-Support1(-d));
    9090                }
    91         btVector3                               Support(const btVector3& d,U index) const
    92                 {
    93                 if(index)
    94                         return(Support1(d));
     91                btVector3                               Support(const btVector3& d,U index) const
     92                {
     93                        if(index)
     94                                return(Support1(d));
    9595                        else
    96                         return(Support0(d));
     96                                return(Support0(d));
    9797                }
    9898        };
    9999
    100 typedef MinkowskiDiff   tShape;
    101 
    102 
    103 // GJK
    104 struct  GJK
    105 {
    106 /* Types                */
    107 struct  sSV
     100        typedef MinkowskiDiff   tShape;
     101
     102
     103        // GJK
     104        struct  GJK
    108105        {
    109         btVector3       d,w;
    110         };
    111 struct  sSimplex
    112         {
    113         sSV*            c[4];
    114         btScalar        p[4];
    115         U                       rank;
    116         };
    117 struct  eStatus { enum _ {
    118         Valid,
    119         Inside,
    120         Failed          };};
    121 /* Fields               */
    122 tShape                  m_shape;
    123 btVector3               m_ray;
    124 btScalar                m_distance;
    125 sSimplex                m_simplices[2];
    126 sSV                             m_store[4];
    127 sSV*                    m_free[4];
    128 U                               m_nfree;
    129 U                               m_current;
    130 sSimplex*               m_simplex;
    131 eStatus::_              m_status;
    132 /* Methods              */
    133                                         GJK()
    134         {
    135         Initialize();
    136         }
    137 void                            Initialize()
    138         {
    139         m_ray           =       btVector3(0,0,0);
    140         m_nfree         =       0;
    141         m_status        =       eStatus::Failed;
    142         m_current       =       0;
    143         m_distance      =       0;
    144         }
    145 eStatus::_                      Evaluate(const tShape& shapearg,const btVector3& guess)
    146         {
    147         U                       iterations=0;
    148         btScalar        sqdist=0;
    149         btScalar        alpha=0;
    150         btVector3       lastw[4];
    151         U                       clastw=0;
    152         /* Initialize solver            */
    153         m_free[0]                       =       &m_store[0];
    154         m_free[1]                       =       &m_store[1];
    155         m_free[2]                       =       &m_store[2];
    156         m_free[3]                       =       &m_store[3];
    157         m_nfree                         =       4;
    158         m_current                       =       0;
    159         m_status                        =       eStatus::Valid;
    160         m_shape                         =       shapearg;
    161         m_distance                      =       0;
    162         /* Initialize simplex           */
    163         m_simplices[0].rank     =       0;
    164         m_ray                           =       guess;
    165         const btScalar  sqrl=   m_ray.length2();
    166         appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
    167         m_simplices[0].p[0]     =       1;
    168         m_ray                           =       m_simplices[0].c[0]->w;
    169         sqdist                          =       sqrl;
    170         lastw[0]                        =
    171         lastw[1]                        =
    172         lastw[2]                        =
    173         lastw[3]                        =       m_ray;
    174         /* Loop                                         */
    175         do      {
    176                 const U         next=1-m_current;
    177                 sSimplex&       cs=m_simplices[m_current];
    178                 sSimplex&       ns=m_simplices[next];
    179                 /* Check zero                                                   */
    180                 const btScalar  rl=m_ray.length();
    181                 if(rl<GJK_MIN_DISTANCE)
    182                         {/* Touching or inside                          */
    183                         m_status=eStatus::Inside;
    184                         break;
    185                         }
    186                 /* Append new vertice in -'v' direction */
    187                 appendvertice(cs,-m_ray);
    188                 const btVector3&        w=cs.c[cs.rank-1]->w;
    189                 bool                            found=false;
    190                 for(U i=0;i<4;++i)
    191                         {
    192                         if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
    193                                 { found=true;break; }
    194                         }
    195                 if(found)
    196                         {/* Return old simplex                          */
    197                         removevertice(m_simplices[m_current]);
    198                         break;
    199                         }
    200                         else
    201                         {/* Update lastw                                        */
    202                         lastw[clastw=(clastw+1)&3]=w;
    203                         }
    204                 /* Check for termination                                */
    205                 const btScalar  omega=dot(m_ray,w)/rl;
    206                 alpha=btMax(omega,alpha);
    207                 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
    208                         {/* Return old simplex                          */
    209                         removevertice(m_simplices[m_current]);
    210                         break;
    211                         }               
    212                 /* Reduce simplex                                               */
    213                 btScalar        weights[4];
    214                 U                       mask=0;
    215                 switch(cs.rank)
    216                         {
    217                         case    2:      sqdist=projectorigin(   cs.c[0]->w,
    218                                                                                                 cs.c[1]->w,
    219                                                                                                 weights,mask);break;
    220                         case    3:      sqdist=projectorigin(   cs.c[0]->w,
    221                                                                                                 cs.c[1]->w,
    222                                                                                                 cs.c[2]->w,
    223                                                                                                 weights,mask);break;
    224                         case    4:      sqdist=projectorigin(   cs.c[0]->w,
    225                                                                                                 cs.c[1]->w,
    226                                                                                                 cs.c[2]->w,
    227                                                                                                 cs.c[3]->w,
    228                                                                                                 weights,mask);break;
    229                         }
    230                 if(sqdist>=0)
    231                         {/* Valid       */
    232                         ns.rank         =       0;
    233                         m_ray           =       btVector3(0,0,0);
    234                         m_current       =       next;
    235                         for(U i=0,ni=cs.rank;i<ni;++i)
    236                                 {
    237                                 if(mask&(1<<i))
    238                                         {
    239                                         ns.c[ns.rank]           =       cs.c[i];
    240                                         ns.p[ns.rank++]         =       weights[i];
    241                                         m_ray                           +=      cs.c[i]->w*weights[i];
     106                /* Types                */
     107                struct  sSV
     108                {
     109                        btVector3       d,w;
     110                };
     111                struct  sSimplex
     112                {
     113                        sSV*            c[4];
     114                        btScalar        p[4];
     115                        U                       rank;
     116                };
     117                struct  eStatus { enum _ {
     118                        Valid,
     119                        Inside,
     120                        Failed          };};
     121                        /* Fields               */
     122                        tShape                  m_shape;
     123                        btVector3               m_ray;
     124                        btScalar                m_distance;
     125                        sSimplex                m_simplices[2];
     126                        sSV                             m_store[4];
     127                        sSV*                    m_free[4];
     128                        U                               m_nfree;
     129                        U                               m_current;
     130                        sSimplex*               m_simplex;
     131                        eStatus::_              m_status;
     132                        /* Methods              */
     133                        GJK()
     134                        {
     135                                Initialize();
     136                        }
     137                        void                            Initialize()
     138                        {
     139                                m_ray           =       btVector3(0,0,0);
     140                                m_nfree         =       0;
     141                                m_status        =       eStatus::Failed;
     142                                m_current       =       0;
     143                                m_distance      =       0;
     144                        }
     145                        eStatus::_                      Evaluate(const tShape& shapearg,const btVector3& guess)
     146                        {
     147                                U                       iterations=0;
     148                                btScalar        sqdist=0;
     149                                btScalar        alpha=0;
     150                                btVector3       lastw[4];
     151                                U                       clastw=0;
     152                                /* Initialize solver            */
     153                                m_free[0]                       =       &m_store[0];
     154                                m_free[1]                       =       &m_store[1];
     155                                m_free[2]                       =       &m_store[2];
     156                                m_free[3]                       =       &m_store[3];
     157                                m_nfree                         =       4;
     158                                m_current                       =       0;
     159                                m_status                        =       eStatus::Valid;
     160                                m_shape                         =       shapearg;
     161                                m_distance                      =       0;
     162                                /* Initialize simplex           */
     163                                m_simplices[0].rank     =       0;
     164                                m_ray                           =       guess;
     165                                const btScalar  sqrl=   m_ray.length2();
     166                                appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
     167                                m_simplices[0].p[0]     =       1;
     168                                m_ray                           =       m_simplices[0].c[0]->w;
     169                                sqdist                          =       sqrl;
     170                                lastw[0]                        =
     171                                        lastw[1]                        =
     172                                        lastw[2]                        =
     173                                        lastw[3]                        =       m_ray;
     174                                /* Loop                                         */
     175                                do      {
     176                                        const U         next=1-m_current;
     177                                        sSimplex&       cs=m_simplices[m_current];
     178                                        sSimplex&       ns=m_simplices[next];
     179                                        /* Check zero                                                   */
     180                                        const btScalar  rl=m_ray.length();
     181                                        if(rl<GJK_MIN_DISTANCE)
     182                                        {/* Touching or inside                          */
     183                                                m_status=eStatus::Inside;
     184                                                break;
     185                                        }
     186                                        /* Append new vertice in -'v' direction */
     187                                        appendvertice(cs,-m_ray);
     188                                        const btVector3&        w=cs.c[cs.rank-1]->w;
     189                                        bool                            found=false;
     190                                        for(U i=0;i<4;++i)
     191                                        {
     192                                                if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
     193                                                { found=true;break; }
     194                                        }
     195                                        if(found)
     196                                        {/* Return old simplex                          */
     197                                                removevertice(m_simplices[m_current]);
     198                                                break;
    242199                                        }
    243200                                        else
    244                                         {
    245                                         m_free[m_nfree++]       =       cs.c[i];
    246                                         }
    247                                 }
    248                         if(mask==15) m_status=eStatus::Inside;
    249                         }
    250                         else
    251                         {/* Return old simplex                          */
    252                         removevertice(m_simplices[m_current]);
    253                         break;
    254                         }
    255                 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
    256                 } while(m_status==eStatus::Valid);
    257         m_simplex=&m_simplices[m_current];
    258         switch(m_status)
    259                 {
    260                 case    eStatus::Valid:         m_distance=m_ray.length();break;
    261                 case    eStatus::Inside:        m_distance=0;break;
    262                 }       
    263         return(m_status);
    264         }
    265 bool                                    EncloseOrigin()
    266         {
    267         switch(m_simplex->rank)
    268                 {
    269                 case    1:
    270                         {
    271                         for(U i=0;i<3;++i)
    272                                 {
    273                                 btVector3               axis=btVector3(0,0,0);
    274                                 axis[i]=1;
    275                                 appendvertice(*m_simplex, axis);
    276                                 if(EncloseOrigin())     return(true);
    277                                 removevertice(*m_simplex);
    278                                 appendvertice(*m_simplex,-axis);
    279                                 if(EncloseOrigin())     return(true);
    280                                 removevertice(*m_simplex);
    281                                 }
    282                         }
    283                 break;
    284                 case    2:
    285                         {
    286                         const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
    287                         for(U i=0;i<3;++i)
    288                                 {
    289                                 btVector3               axis=btVector3(0,0,0);
    290                                 axis[i]=1;
    291                                 const btVector3 p=cross(d,axis);
    292                                 if(p.length2()>0)
    293                                         {
    294                                         appendvertice(*m_simplex, p);
    295                                         if(EncloseOrigin())     return(true);
    296                                         removevertice(*m_simplex);
    297                                         appendvertice(*m_simplex,-p);
    298                                         if(EncloseOrigin())     return(true);
    299                                         removevertice(*m_simplex);
    300                                         }
    301                                 }
    302                         }
    303                 break;
    304                 case    3:
    305                         {
    306                         const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
    307                                                                         m_simplex->c[2]->w-m_simplex->c[0]->w);
    308                         if(n.length2()>0)
    309                                 {
    310                                 appendvertice(*m_simplex,n);
    311                                 if(EncloseOrigin())     return(true);
    312                                 removevertice(*m_simplex);
    313                                 appendvertice(*m_simplex,-n);
    314                                 if(EncloseOrigin())     return(true);
    315                                 removevertice(*m_simplex);
    316                                 }
    317                         }
    318                 break;
    319                 case    4:
    320                         {
    321                         if(btFabs(det(  m_simplex->c[0]->w-m_simplex->c[3]->w,
     201                                        {/* Update lastw                                        */
     202                                                lastw[clastw=(clastw+1)&3]=w;
     203                                        }
     204                                        /* Check for termination                                */
     205                                        const btScalar  omega=dot(m_ray,w)/rl;
     206                                        alpha=btMax(omega,alpha);
     207                                        if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
     208                                        {/* Return old simplex                          */
     209                                                removevertice(m_simplices[m_current]);
     210                                                break;
     211                                        }               
     212                                        /* Reduce simplex                                               */
     213                                        btScalar        weights[4];
     214                                        U                       mask=0;
     215                                        switch(cs.rank)
     216                                        {
     217                                        case    2:      sqdist=projectorigin(   cs.c[0]->w,
     218                                                                        cs.c[1]->w,
     219                                                                        weights,mask);break;
     220                                        case    3:      sqdist=projectorigin(   cs.c[0]->w,
     221                                                                        cs.c[1]->w,
     222                                                                        cs.c[2]->w,
     223                                                                        weights,mask);break;
     224                                        case    4:      sqdist=projectorigin(   cs.c[0]->w,
     225                                                                        cs.c[1]->w,
     226                                                                        cs.c[2]->w,
     227                                                                        cs.c[3]->w,
     228                                                                        weights,mask);break;
     229                                        }
     230                                        if(sqdist>=0)
     231                                        {/* Valid       */
     232                                                ns.rank         =       0;
     233                                                m_ray           =       btVector3(0,0,0);
     234                                                m_current       =       next;
     235                                                for(U i=0,ni=cs.rank;i<ni;++i)
     236                                                {
     237                                                        if(mask&(1<<i))
     238                                                        {
     239                                                                ns.c[ns.rank]           =       cs.c[i];
     240                                                                ns.p[ns.rank++]         =       weights[i];
     241                                                                m_ray                           +=      cs.c[i]->w*weights[i];
     242                                                        }
     243                                                        else
     244                                                        {
     245                                                                m_free[m_nfree++]       =       cs.c[i];
     246                                                        }
     247                                                }
     248                                                if(mask==15) m_status=eStatus::Inside;
     249                                        }
     250                                        else
     251                                        {/* Return old simplex                          */
     252                                                removevertice(m_simplices[m_current]);
     253                                                break;
     254                                        }
     255                                        m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
     256                                } while(m_status==eStatus::Valid);
     257                                m_simplex=&m_simplices[m_current];
     258                                switch(m_status)
     259                                {
     260                                case    eStatus::Valid:         m_distance=m_ray.length();break;
     261                                case    eStatus::Inside:        m_distance=0;break;
     262                                }       
     263                                return(m_status);
     264                        }
     265                        bool                                    EncloseOrigin()
     266                        {
     267                                switch(m_simplex->rank)
     268                                {
     269                                case    1:
     270                                        {
     271                                                for(U i=0;i<3;++i)
     272                                                {
     273                                                        btVector3               axis=btVector3(0,0,0);
     274                                                        axis[i]=1;
     275                                                        appendvertice(*m_simplex, axis);
     276                                                        if(EncloseOrigin())     return(true);
     277                                                        removevertice(*m_simplex);
     278                                                        appendvertice(*m_simplex,-axis);
     279                                                        if(EncloseOrigin())     return(true);
     280                                                        removevertice(*m_simplex);
     281                                                }
     282                                        }
     283                                        break;
     284                                case    2:
     285                                        {
     286                                                const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
     287                                                for(U i=0;i<3;++i)
     288                                                {
     289                                                        btVector3               axis=btVector3(0,0,0);
     290                                                        axis[i]=1;
     291                                                        const btVector3 p=cross(d,axis);
     292                                                        if(p.length2()>0)
     293                                                        {
     294                                                                appendvertice(*m_simplex, p);
     295                                                                if(EncloseOrigin())     return(true);
     296                                                                removevertice(*m_simplex);
     297                                                                appendvertice(*m_simplex,-p);
     298                                                                if(EncloseOrigin())     return(true);
     299                                                                removevertice(*m_simplex);
     300                                                        }
     301                                                }
     302                                        }
     303                                        break;
     304                                case    3:
     305                                        {
     306                                                const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
     307                                                        m_simplex->c[2]->w-m_simplex->c[0]->w);
     308                                                if(n.length2()>0)
     309                                                {
     310                                                        appendvertice(*m_simplex,n);
     311                                                        if(EncloseOrigin())     return(true);
     312                                                        removevertice(*m_simplex);
     313                                                        appendvertice(*m_simplex,-n);
     314                                                        if(EncloseOrigin())     return(true);
     315                                                        removevertice(*m_simplex);
     316                                                }
     317                                        }
     318                                        break;
     319                                case    4:
     320                                        {
     321                                                if(btFabs(det(  m_simplex->c[0]->w-m_simplex->c[3]->w,
    322322                                                        m_simplex->c[1]->w-m_simplex->c[3]->w,
    323323                                                        m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
    324                                 return(true);
    325                         }
    326                 break;
    327                 }
    328         return(false);
    329         }
    330 /* Internals    */
    331 void                            getsupport(const btVector3& d,sSV& sv) const
    332         {
    333         sv.d    =       d/d.length();
    334         sv.w    =       m_shape.Support(sv.d);
    335         }
    336 void                            removevertice(sSimplex& simplex)
    337         {
    338         m_free[m_nfree++]=simplex.c[--simplex.rank];
    339         }
    340 void                            appendvertice(sSimplex& simplex,const btVector3& v)
    341         {
    342         simplex.p[simplex.rank]=0;
    343         simplex.c[simplex.rank]=m_free[--m_nfree];
    344         getsupport(v,*simplex.c[simplex.rank++]);
    345         }
    346 static btScalar         det(const btVector3& a,const btVector3& b,const btVector3& c)
    347         {
    348         return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
    349                         a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
    350                         a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
    351         }
    352 static btScalar         projectorigin(  const btVector3& a,
    353                                                                         const btVector3& b,
    354                                                                         btScalar* w,U& m)
    355         {
    356         const btVector3 d=b-a;
    357         const btScalar  l=d.length2();
    358         if(l>GJK_SIMPLEX2_EPS)
    359                 {
    360                 const btScalar  t(l>0?-dot(a,d)/l:0);
    361                 if(t>=1)                { w[0]=0;w[1]=1;m=2;return(b.length2()); }
    362                 else if(t<=0)   { w[0]=1;w[1]=0;m=1;return(a.length2()); }
    363                 else                    { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
    364                 }
    365         return(-1);
    366         }
    367 static btScalar         projectorigin(  const btVector3& a,
    368                                                                         const btVector3& b,
    369                                                                         const btVector3& c,
    370                                                                         btScalar* w,U& m)
    371         {
    372         static const U          imd3[]={1,2,0};
    373         const btVector3*        vt[]={&a,&b,&c};
    374         const btVector3         dl[]={a-b,b-c,c-a};
    375         const btVector3         n=cross(dl[0],dl[1]);
    376         const btScalar          l=n.length2();
    377         if(l>GJK_SIMPLEX3_EPS)
    378                 {
    379                 btScalar        mindist=-1;
    380                 btScalar        subw[2];
    381                 U                       subm;
    382                 for(U i=0;i<3;++i)
    383                         {
    384                         if(dot(*vt[i],cross(dl[i],n))>0)
    385                                 {
    386                                 const U                 j=imd3[i];
    387                                 const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
    388                                 if((mindist<0)||(subd<mindist))
    389                                         {
    390                                         mindist         =       subd;
    391                                         m                       =       static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
    392                                         w[i]            =       subw[0];
    393                                         w[j]            =       subw[1];
    394                                         w[imd3[j]]      =       0;                             
    395                                         }
     324                                                        return(true);
     325                                        }
     326                                        break;
    396327                                }
    397                         }
    398                 if(mindist<0)
    399                         {
    400                         const btScalar  d=dot(a,n);     
    401                         const btScalar  s=btSqrt(l);
    402                         const btVector3 p=n*(d/l);
    403                         mindist =       p.length2();
    404                         m               =       7;
    405                         w[0]    =       (cross(dl[1],b-p)).length()/s;
    406                         w[1]    =       (cross(dl[2],c-p)).length()/s;
    407                         w[2]    =       1-(w[0]+w[1]);
    408                         }
    409                 return(mindist);
    410                 }
    411         return(-1);
    412         }
    413 static btScalar         projectorigin(  const btVector3& a,
    414                                                                         const btVector3& b,
    415                                                                         const btVector3& c,
    416                                                                         const btVector3& d,
    417                                                                         btScalar* w,U& m)
    418         {
    419         static const U          imd3[]={1,2,0};
    420         const btVector3*        vt[]={&a,&b,&c,&d};
    421         const btVector3         dl[]={a-d,b-d,c-d};
    422         const btScalar          vl=det(dl[0],dl[1],dl[2]);
    423         const bool                      ng=(vl*dot(a,cross(b-c,a-b)))<=0;
    424         if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
    425                 {
    426                 btScalar        mindist=-1;
    427                 btScalar        subw[3];
    428                 U                       subm;
    429                 for(U i=0;i<3;++i)
    430                         {
    431                         const U                 j=imd3[i];
    432                         const btScalar  s=vl*dot(d,cross(dl[i],dl[j]));
    433                         if(s>0)
    434                                 {
    435                                 const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
    436                                 if((mindist<0)||(subd<mindist))
    437                                         {
    438                                         mindist         =       subd;
    439                                         m                       =       static_cast<U>((subm&1?1<<i:0)+
     328                                return(false);
     329                        }
     330                        /* Internals    */
     331                        void                            getsupport(const btVector3& d,sSV& sv) const
     332                        {
     333                                sv.d    =       d/d.length();
     334                                sv.w    =       m_shape.Support(sv.d);
     335                        }
     336                        void                            removevertice(sSimplex& simplex)
     337                        {
     338                                m_free[m_nfree++]=simplex.c[--simplex.rank];
     339                        }
     340                        void                            appendvertice(sSimplex& simplex,const btVector3& v)
     341                        {
     342                                simplex.p[simplex.rank]=0;
     343                                simplex.c[simplex.rank]=m_free[--m_nfree];
     344                                getsupport(v,*simplex.c[simplex.rank++]);
     345                        }
     346                        static btScalar         det(const btVector3& a,const btVector3& b,const btVector3& c)
     347                        {
     348                                return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
     349                                        a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
     350                                        a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
     351                        }
     352                        static btScalar         projectorigin(  const btVector3& a,
     353                                const btVector3& b,
     354                                btScalar* w,U& m)
     355                        {
     356                                const btVector3 d=b-a;
     357                                const btScalar  l=d.length2();
     358                                if(l>GJK_SIMPLEX2_EPS)
     359                                {
     360                                        const btScalar  t(l>0?-dot(a,d)/l:0);
     361                                        if(t>=1)                { w[0]=0;w[1]=1;m=2;return(b.length2()); }
     362                                        else if(t<=0)   { w[0]=1;w[1]=0;m=1;return(a.length2()); }
     363                                        else                    { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
     364                                }
     365                                return(-1);
     366                        }
     367                        static btScalar         projectorigin(  const btVector3& a,
     368                                const btVector3& b,
     369                                const btVector3& c,
     370                                btScalar* w,U& m)
     371                        {
     372                                static const U          imd3[]={1,2,0};
     373                                const btVector3*        vt[]={&a,&b,&c};
     374                                const btVector3         dl[]={a-b,b-c,c-a};
     375                                const btVector3         n=cross(dl[0],dl[1]);
     376                                const btScalar          l=n.length2();
     377                                if(l>GJK_SIMPLEX3_EPS)
     378                                {
     379                                        btScalar        mindist=-1;
     380                                        btScalar        subw[2];
     381                                        U                       subm;
     382                                        for(U i=0;i<3;++i)
     383                                        {
     384                                                if(dot(*vt[i],cross(dl[i],n))>0)
     385                                                {
     386                                                        const U                 j=imd3[i];
     387                                                        const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
     388                                                        if((mindist<0)||(subd<mindist))
     389                                                        {
     390                                                                mindist         =       subd;
     391                                                                m                       =       static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
     392                                                                w[i]            =       subw[0];
     393                                                                w[j]            =       subw[1];
     394                                                                w[imd3[j]]      =       0;                             
     395                                                        }
     396                                                }
     397                                        }
     398                                        if(mindist<0)
     399                                        {
     400                                                const btScalar  d=dot(a,n);     
     401                                                const btScalar  s=btSqrt(l);
     402                                                const btVector3 p=n*(d/l);
     403                                                mindist =       p.length2();
     404                                                m               =       7;
     405                                                w[0]    =       (cross(dl[1],b-p)).length()/s;
     406                                                w[1]    =       (cross(dl[2],c-p)).length()/s;
     407                                                w[2]    =       1-(w[0]+w[1]);
     408                                        }
     409                                        return(mindist);
     410                                }
     411                                return(-1);
     412                        }
     413                        static btScalar         projectorigin(  const btVector3& a,
     414                                const btVector3& b,
     415                                const btVector3& c,
     416                                const btVector3& d,
     417                                btScalar* w,U& m)
     418                        {
     419                                static const U          imd3[]={1,2,0};
     420                                const btVector3*        vt[]={&a,&b,&c,&d};
     421                                const btVector3         dl[]={a-d,b-d,c-d};
     422                                const btScalar          vl=det(dl[0],dl[1],dl[2]);
     423                                const bool                      ng=(vl*dot(a,cross(b-c,a-b)))<=0;
     424                                if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
     425                                {
     426                                        btScalar        mindist=-1;
     427                                        btScalar        subw[3];
     428                                        U                       subm;
     429                                        for(U i=0;i<3;++i)
     430                                        {
     431                                                const U                 j=imd3[i];
     432                                                const btScalar  s=vl*dot(d,cross(dl[i],dl[j]));
     433                                                if(s>0)
     434                                                {
     435                                                        const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
     436                                                        if((mindist<0)||(subd<mindist))
     437                                                        {
     438                                                                mindist         =       subd;
     439                                                                m                       =       static_cast<U>((subm&1?1<<i:0)+
    440440                                                                        (subm&2?1<<j:0)+
    441441                                                                        (subm&4?8:0));
    442                                         w[i]            =       subw[0];
    443                                         w[j]            =       subw[1];
    444                                         w[imd3[j]]      =       0;
    445                                         w[3]            =       subw[2];
    446                                         }
     442                                                                w[i]            =       subw[0];
     443                                                                w[j]            =       subw[1];
     444                                                                w[imd3[j]]      =       0;
     445                                                                w[3]            =       subw[2];
     446                                                        }
     447                                                }
     448                                        }
     449                                        if(mindist<0)
     450                                        {
     451                                                mindist =       0;
     452                                                m               =       15;
     453                                                w[0]    =       det(c,b,d)/vl;
     454                                                w[1]    =       det(a,c,d)/vl;
     455                                                w[2]    =       det(b,a,d)/vl;
     456                                                w[3]    =       1-(w[0]+w[1]+w[2]);
     457                                        }
     458                                        return(mindist);
    447459                                }
    448                         }
    449                 if(mindist<0)
    450                         {
    451                         mindist =       0;
    452                         m               =       15;
    453                         w[0]    =       det(c,b,d)/vl;
    454                         w[1]    =       det(a,c,d)/vl;
    455                         w[2]    =       det(b,a,d)/vl;
    456                         w[3]    =       1-(w[0]+w[1]+w[2]);
    457                         }
    458                 return(mindist);
    459                 }
    460         return(-1);
    461         }
    462 };
    463 
    464 // EPA
    465 struct  EPA
    466 {
    467 /* Types                */
    468 typedef GJK::sSV        sSV;
    469 struct  sFace
     460                                return(-1);
     461                        }
     462        };
     463
     464        // EPA
     465        struct  EPA
    470466        {
    471         btVector3       n;
    472         btScalar        d;
    473         btScalar        p;
    474         sSV*            c[3];
    475         sFace*          f[3];
    476         sFace*          l[2];
    477         U1                      e[3];
    478         U1                      pass;
    479         };
    480 struct  sList
    481         {
    482         sFace*          root;
    483         U                       count;
    484                                 sList() : root(0),count(0)      {}
    485         };
    486 struct  sHorizon
    487         {
    488         sFace*          cf;
    489         sFace*          ff;
    490         U                       nf;
    491                                 sHorizon() : cf(0),ff(0),nf(0)  {}
    492         };
    493 struct  eStatus { enum _ {
    494         Valid,
    495         Touching,
    496         Degenerated,
    497         NonConvex,
    498         InvalidHull,           
    499         OutOfFaces,
    500         OutOfVertices,
    501         AccuraryReached,
    502         FallBack,
    503         Failed          };};
    504 /* Fields               */
    505 eStatus::_              m_status;
    506 GJK::sSimplex   m_result;
    507 btVector3               m_normal;
    508 btScalar                m_depth;
    509 sSV                             m_sv_store[EPA_MAX_VERTICES];
    510 sFace                   m_fc_store[EPA_MAX_FACES];
    511 U                               m_nextsv;
    512 sList                   m_hull;
    513 sList                   m_stock;
    514 /* Methods              */
    515                                         EPA()
    516         {
    517         Initialize();   
    518         }
    519 
    520 
    521                                         static inline void              bind(sFace* fa,U ea,sFace* fb,U eb)
    522         {
    523         fa->e[ea]=(U1)eb;fa->f[ea]=fb;
    524         fb->e[eb]=(U1)ea;fb->f[eb]=fa;
    525         }
    526 static inline void              append(sList& list,sFace* face)
    527         {
    528         face->l[0]      =       0;
    529         face->l[1]      =       list.root;
    530         if(list.root) list.root->l[0]=face;
    531         list.root       =       face;
    532         ++list.count;
    533         }
    534 static inline void              remove(sList& list,sFace* face)
    535         {
    536         if(face->l[1]) face->l[1]->l[0]=face->l[0];
    537         if(face->l[0]) face->l[0]->l[1]=face->l[1];
    538         if(face==list.root) list.root=face->l[1];
    539         --list.count;
    540         }
    541 
    542 
    543 void                            Initialize()
    544         {
    545         m_status        =       eStatus::Failed;
    546         m_normal        =       btVector3(0,0,0);
    547         m_depth         =       0;
    548         m_nextsv        =       0;
    549         for(U i=0;i<EPA_MAX_FACES;++i)
    550                 {
    551                 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
    552                 }
    553         }
    554 eStatus::_                      Evaluate(GJK& gjk,const btVector3& guess)
    555         {
    556         GJK::sSimplex&  simplex=*gjk.m_simplex;
    557         if((simplex.rank>1)&&gjk.EncloseOrigin())
    558                 {
    559 
    560                 /* Clean up                             */
    561                 while(m_hull.root)
    562                         {
    563                         sFace*  f = m_hull.root;
    564                         remove(m_hull,f);
    565                         append(m_stock,f);
    566                         }
    567                 m_status        =       eStatus::Valid;
    568                 m_nextsv        =       0;
    569                 /* Orient simplex               */
    570                 if(gjk.det(     simplex.c[0]->w-simplex.c[3]->w,
    571                                         simplex.c[1]->w-simplex.c[3]->w,
    572                                         simplex.c[2]->w-simplex.c[3]->w)<0)
    573                         {
    574                         btSwap(simplex.c[0],simplex.c[1]);
    575                         btSwap(simplex.p[0],simplex.p[1]);
    576                         }
    577                 /* Build initial hull   */
    578                 sFace*  tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
     467                /* Types                */
     468                typedef GJK::sSV        sSV;
     469                struct  sFace
     470                {
     471                        btVector3       n;
     472                        btScalar        d;
     473                        btScalar        p;
     474                        sSV*            c[3];
     475                        sFace*          f[3];
     476                        sFace*          l[2];
     477                        U1                      e[3];
     478                        U1                      pass;
     479                };
     480                struct  sList
     481                {
     482                        sFace*          root;
     483                        U                       count;
     484                        sList() : root(0),count(0)      {}
     485                };
     486                struct  sHorizon
     487                {
     488                        sFace*          cf;
     489                        sFace*          ff;
     490                        U                       nf;
     491                        sHorizon() : cf(0),ff(0),nf(0)  {}
     492                };
     493                struct  eStatus { enum _ {
     494                        Valid,
     495                        Touching,
     496                        Degenerated,
     497                        NonConvex,
     498                        InvalidHull,           
     499                        OutOfFaces,
     500                        OutOfVertices,
     501                        AccuraryReached,
     502                        FallBack,
     503                        Failed          };};
     504                        /* Fields               */
     505                        eStatus::_              m_status;
     506                        GJK::sSimplex   m_result;
     507                        btVector3               m_normal;
     508                        btScalar                m_depth;
     509                        sSV                             m_sv_store[EPA_MAX_VERTICES];
     510                        sFace                   m_fc_store[EPA_MAX_FACES];
     511                        U                               m_nextsv;
     512                        sList                   m_hull;
     513                        sList                   m_stock;
     514                        /* Methods              */
     515                        EPA()
     516                        {
     517                                Initialize();   
     518                        }
     519
     520
     521                        static inline void              bind(sFace* fa,U ea,sFace* fb,U eb)
     522                        {
     523                                fa->e[ea]=(U1)eb;fa->f[ea]=fb;
     524                                fb->e[eb]=(U1)ea;fb->f[eb]=fa;
     525                        }
     526                        static inline void              append(sList& list,sFace* face)
     527                        {
     528                                face->l[0]      =       0;
     529                                face->l[1]      =       list.root;
     530                                if(list.root) list.root->l[0]=face;
     531                                list.root       =       face;
     532                                ++list.count;
     533                        }
     534                        static inline void              remove(sList& list,sFace* face)
     535                        {
     536                                if(face->l[1]) face->l[1]->l[0]=face->l[0];
     537                                if(face->l[0]) face->l[0]->l[1]=face->l[1];
     538                                if(face==list.root) list.root=face->l[1];
     539                                --list.count;
     540                        }
     541
     542
     543                        void                            Initialize()
     544                        {
     545                                m_status        =       eStatus::Failed;
     546                                m_normal        =       btVector3(0,0,0);
     547                                m_depth         =       0;
     548                                m_nextsv        =       0;
     549                                for(U i=0;i<EPA_MAX_FACES;++i)
     550                                {
     551                                        append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
     552                                }
     553                        }
     554                        eStatus::_                      Evaluate(GJK& gjk,const btVector3& guess)
     555                        {
     556                                GJK::sSimplex&  simplex=*gjk.m_simplex;
     557                                if((simplex.rank>1)&&gjk.EncloseOrigin())
     558                                {
     559
     560                                        /* Clean up                             */
     561                                        while(m_hull.root)
     562                                        {
     563                                                sFace*  f = m_hull.root;
     564                                                remove(m_hull,f);
     565                                                append(m_stock,f);
     566                                        }
     567                                        m_status        =       eStatus::Valid;
     568                                        m_nextsv        =       0;
     569                                        /* Orient simplex               */
     570                                        if(gjk.det(     simplex.c[0]->w-simplex.c[3]->w,
     571                                                simplex.c[1]->w-simplex.c[3]->w,
     572                                                simplex.c[2]->w-simplex.c[3]->w)<0)
     573                                        {
     574                                                btSwap(simplex.c[0],simplex.c[1]);
     575                                                btSwap(simplex.p[0],simplex.p[1]);
     576                                        }
     577                                        /* Build initial hull   */
     578                                        sFace*  tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
    579579                                                newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
    580580                                                newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
    581581                                                newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
    582                 if(m_hull.count==4)
    583                         {
    584                         sFace*          best=findbest();
    585                         sFace           outer=*best;
    586                         U                       pass=0;
    587                         U                       iterations=0;
    588                         bind(tetra[0],0,tetra[1],0);
    589                         bind(tetra[0],1,tetra[2],0);
    590                         bind(tetra[0],2,tetra[3],0);
    591                         bind(tetra[1],1,tetra[3],2);
    592                         bind(tetra[1],2,tetra[2],1);
    593                         bind(tetra[2],2,tetra[3],1);
    594                         m_status=eStatus::Valid;
    595                         for(;iterations<EPA_MAX_ITERATIONS;++iterations)
    596                                 {
    597                                 if(m_nextsv<EPA_MAX_VERTICES)
    598                                         {       
    599                                         sHorizon                horizon;
    600                                         sSV*                    w=&m_sv_store[m_nextsv++];
    601                                         bool                    valid=true;                                     
    602                                         best->pass      =       (U1)(++pass);
    603                                         gjk.getsupport(best->n,*w);
    604                                         const btScalar  wdist=dot(best->n,w->w)-best->d;
    605                                         if(wdist>EPA_ACCURACY)
     582                                        if(m_hull.count==4)
     583                                        {
     584                                                sFace*          best=findbest();
     585                                                sFace           outer=*best;
     586                                                U                       pass=0;
     587                                                U                       iterations=0;
     588                                                bind(tetra[0],0,tetra[1],0);
     589                                                bind(tetra[0],1,tetra[2],0);
     590                                                bind(tetra[0],2,tetra[3],0);
     591                                                bind(tetra[1],1,tetra[3],2);
     592                                                bind(tetra[1],2,tetra[2],1);
     593                                                bind(tetra[2],2,tetra[3],1);
     594                                                m_status=eStatus::Valid;
     595                                                for(;iterations<EPA_MAX_ITERATIONS;++iterations)
    606596                                                {
    607                                                 for(U j=0;(j<3)&&valid;++j)
    608                                                         {
    609                                                         valid&=expand(  pass,w,
     597                                                        if(m_nextsv<EPA_MAX_VERTICES)
     598                                                        {       
     599                                                                sHorizon                horizon;
     600                                                                sSV*                    w=&m_sv_store[m_nextsv++];
     601                                                                bool                    valid=true;                                     
     602                                                                best->pass      =       (U1)(++pass);
     603                                                                gjk.getsupport(best->n,*w);
     604                                                                const btScalar  wdist=dot(best->n,w->w)-best->d;
     605                                                                if(wdist>EPA_ACCURACY)
     606                                                                {
     607                                                                        for(U j=0;(j<3)&&valid;++j)
     608                                                                        {
     609                                                                                valid&=expand(  pass,w,
    610610                                                                                        best->f[j],best->e[j],
    611611                                                                                        horizon);
    612                                                         }
    613                                                 if(valid&&(horizon.nf>=3))
    614                                                         {
    615                                                         bind(horizon.cf,1,horizon.ff,2);
    616                                                         remove(m_hull,best);
    617                                                         append(m_stock,best);
    618                                                         best=findbest();
    619                                                         if(best->p>=outer.p) outer=*best;
    620                                                         } else { m_status=eStatus::InvalidHull;break; }
    621                                                 } else { m_status=eStatus::AccuraryReached;break; }
    622                                         } else { m_status=eStatus::OutOfVertices;break; }
     612                                                                        }
     613                                                                        if(valid&&(horizon.nf>=3))
     614                                                                        {
     615                                                                                bind(horizon.cf,1,horizon.ff,2);
     616                                                                                remove(m_hull,best);
     617                                                                                append(m_stock,best);
     618                                                                                best=findbest();
     619                                                                                if(best->p>=outer.p) outer=*best;
     620                                                                        } else { m_status=eStatus::InvalidHull;break; }
     621                                                                } else { m_status=eStatus::AccuraryReached;break; }
     622                                                        } else { m_status=eStatus::OutOfVertices;break; }
     623                                                }
     624                                                const btVector3 projection=outer.n*outer.d;
     625                                                m_normal        =       outer.n;
     626                                                m_depth         =       outer.d;
     627                                                m_result.rank   =       3;
     628                                                m_result.c[0]   =       outer.c[0];
     629                                                m_result.c[1]   =       outer.c[1];
     630                                                m_result.c[2]   =       outer.c[2];
     631                                                m_result.p[0]   =       cross(  outer.c[1]->w-projection,
     632                                                        outer.c[2]->w-projection).length();
     633                                                m_result.p[1]   =       cross(  outer.c[2]->w-projection,
     634                                                        outer.c[0]->w-projection).length();
     635                                                m_result.p[2]   =       cross(  outer.c[0]->w-projection,
     636                                                        outer.c[1]->w-projection).length();
     637                                                const btScalar  sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
     638                                                m_result.p[0]   /=      sum;
     639                                                m_result.p[1]   /=      sum;
     640                                                m_result.p[2]   /=      sum;
     641                                                return(m_status);
     642                                        }
    623643                                }
    624                         const btVector3 projection=outer.n*outer.d;
    625                         m_normal        =       outer.n;
    626                         m_depth         =       outer.d;
    627                         m_result.rank   =       3;
    628                         m_result.c[0]   =       outer.c[0];
    629                         m_result.c[1]   =       outer.c[1];
    630                         m_result.c[2]   =       outer.c[2];
    631                         m_result.p[0]   =       cross(  outer.c[1]->w-projection,
    632                                                                                 outer.c[2]->w-projection).length();
    633                         m_result.p[1]   =       cross(  outer.c[2]->w-projection,
    634                                                                                 outer.c[0]->w-projection).length();
    635                         m_result.p[2]   =       cross(  outer.c[0]->w-projection,
    636                                                                                 outer.c[1]->w-projection).length();
    637                         const btScalar  sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
    638                         m_result.p[0]   /=      sum;
    639                         m_result.p[1]   /=      sum;
    640                         m_result.p[2]   /=      sum;
    641                         return(m_status);
    642                         }
    643                 }
    644         /* Fallback             */
    645         m_status        =       eStatus::FallBack;
    646         m_normal        =       -guess;
    647         const btScalar  nl=m_normal.length();
    648         if(nl>0)
    649                 m_normal        =       m_normal/nl;
    650                 else
    651                 m_normal        =       btVector3(1,0,0);
    652         m_depth =       0;
    653         m_result.rank=1;
    654         m_result.c[0]=simplex.c[0];
    655         m_result.p[0]=1;       
    656         return(m_status);
     644                                /* Fallback             */
     645                                m_status        =       eStatus::FallBack;
     646                                m_normal        =       -guess;
     647                                const btScalar  nl=m_normal.length();
     648                                if(nl>0)
     649                                        m_normal        =       m_normal/nl;
     650                                else
     651                                        m_normal        =       btVector3(1,0,0);
     652                                m_depth =       0;
     653                                m_result.rank=1;
     654                                m_result.c[0]=simplex.c[0];
     655                                m_result.p[0]=1;       
     656                                return(m_status);
     657                        }
     658                        sFace*                          newface(sSV* a,sSV* b,sSV* c,bool forced)
     659                        {
     660                                if(m_stock.root)
     661                                {
     662                                        sFace*  face=m_stock.root;
     663                                        remove(m_stock,face);
     664                                        append(m_hull,face);
     665                                        face->pass      =       0;
     666                                        face->c[0]      =       a;
     667                                        face->c[1]      =       b;
     668                                        face->c[2]      =       c;
     669                                        face->n         =       cross(b->w-a->w,c->w-a->w);
     670                                        const btScalar  l=face->n.length();
     671                                        const bool              v=l>EPA_ACCURACY;
     672                                        face->p         =       btMin(btMin(
     673                                                dot(a->w,cross(face->n,a->w-b->w)),
     674                                                dot(b->w,cross(face->n,b->w-c->w))),
     675                                                dot(c->w,cross(face->n,c->w-a->w)))     /
     676                                                (v?l:1);
     677                                        face->p         =       face->p>=-EPA_INSIDE_EPS?0:face->p;
     678                                        if(v)
     679                                        {
     680                                                face->d         =       dot(a->w,face->n)/l;
     681                                                face->n         /=      l;
     682                                                if(forced||(face->d>=-EPA_PLANE_EPS))
     683                                                {
     684                                                        return(face);
     685                                                } else m_status=eStatus::NonConvex;
     686                                        } else m_status=eStatus::Degenerated;
     687                                        remove(m_hull,face);
     688                                        append(m_stock,face);
     689                                        return(0);
     690                                }
     691                                m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
     692                                return(0);
     693                        }
     694                        sFace*                          findbest()
     695                        {
     696                                sFace*          minf=m_hull.root;
     697                                btScalar        mind=minf->d*minf->d;
     698                                btScalar        maxp=minf->p;
     699                                for(sFace* f=minf->l[1];f;f=f->l[1])
     700                                {
     701                                        const btScalar  sqd=f->d*f->d;
     702                                        if((f->p>=maxp)&&(sqd<mind))
     703                                        {
     704                                                minf=f;
     705                                                mind=sqd;
     706                                                maxp=f->p;
     707                                        }
     708                                }
     709                                return(minf);
     710                        }
     711                        bool                            expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
     712                        {
     713                                static const U  i1m3[]={1,2,0};
     714                                static const U  i2m3[]={2,0,1};
     715                                if(f->pass!=pass)
     716                                {
     717                                        const U e1=i1m3[e];
     718                                        if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
     719                                        {
     720                                                sFace*  nf=newface(f->c[e1],f->c[e],w,false);
     721                                                if(nf)
     722                                                {
     723                                                        bind(nf,0,f,e);
     724                                                        if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
     725                                                        horizon.cf=nf;
     726                                                        ++horizon.nf;
     727                                                        return(true);
     728                                                }
     729                                        }
     730                                        else
     731                                        {
     732                                                const U e2=i2m3[e];
     733                                                f->pass         =       (U1)pass;
     734                                                if(     expand(pass,w,f->f[e1],f->e[e1],horizon)&&
     735                                                        expand(pass,w,f->f[e2],f->e[e2],horizon))
     736                                                {
     737                                                        remove(m_hull,f);
     738                                                        append(m_stock,f);
     739                                                        return(true);
     740                                                }
     741                                        }
     742                                }
     743                                return(false);
     744                        }
     745
     746        };
     747
     748        //
     749        static void     Initialize(     const btConvexShape* shape0,const btTransform& wtrs0,
     750                const btConvexShape* shape1,const btTransform& wtrs1,
     751                btGjkEpaSolver2::sResults& results,
     752                tShape& shape,
     753                bool withmargins)
     754        {
     755                /* Results              */
     756                results.witnesses[0]    =
     757                        results.witnesses[1]    =       btVector3(0,0,0);
     758                results.status                  =       btGjkEpaSolver2::sResults::Separated;
     759                /* Shape                */
     760                shape.m_shapes[0]               =       shape0;
     761                shape.m_shapes[1]               =       shape1;
     762                shape.m_toshape1                =       wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
     763                shape.m_toshape0                =       wtrs0.inverseTimes(wtrs1);
     764                shape.EnableMargin(withmargins);
    657765        }
    658 sFace*                          newface(sSV* a,sSV* b,sSV* c,bool forced)
    659         {
    660         if(m_stock.root)
    661                 {
    662                 sFace*  face=m_stock.root;
    663                 remove(m_stock,face);
    664                 append(m_hull,face);
    665                 face->pass      =       0;
    666                 face->c[0]      =       a;
    667                 face->c[1]      =       b;
    668                 face->c[2]      =       c;
    669                 face->n         =       cross(b->w-a->w,c->w-a->w);
    670                 const btScalar  l=face->n.length();
    671                 const bool              v=l>EPA_ACCURACY;
    672                 face->p         =       btMin(btMin(
    673                                                         dot(a->w,cross(face->n,a->w-b->w)),
    674                                                         dot(b->w,cross(face->n,b->w-c->w))),
    675                                                         dot(c->w,cross(face->n,c->w-a->w)))     /
    676                                                         (v?l:1);
    677                 face->p         =       face->p>=-EPA_INSIDE_EPS?0:face->p;
    678                 if(v)
    679                         {
    680                         face->d         =       dot(a->w,face->n)/l;
    681                         face->n         /=      l;
    682                         if(forced||(face->d>=-EPA_PLANE_EPS))
    683                                 {
    684                                 return(face);
    685                                 } else m_status=eStatus::NonConvex;
    686                         } else m_status=eStatus::Degenerated;
    687                 remove(m_hull,face);
    688                 append(m_stock,face);
    689                 return(0);
    690                 }
    691         m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
    692         return(0);
    693         }
    694 sFace*                          findbest()
    695         {
    696         sFace*          minf=m_hull.root;
    697         btScalar        mind=minf->d*minf->d;
    698         btScalar        maxp=minf->p;
    699         for(sFace* f=minf->l[1];f;f=f->l[1])
    700                 {
    701                 const btScalar  sqd=f->d*f->d;
    702                 if((f->p>=maxp)&&(sqd<mind))
    703                         {
    704                         minf=f;
    705                         mind=sqd;
    706                         maxp=f->p;
    707                         }
    708                 }
    709         return(minf);
    710         }
    711 bool                            expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
    712         {
    713         static const U  i1m3[]={1,2,0};
    714         static const U  i2m3[]={2,0,1};
    715         if(f->pass!=pass)
    716                 {
    717                 const U e1=i1m3[e];
    718                 if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
    719                         {
    720                         sFace*  nf=newface(f->c[e1],f->c[e],w,false);
    721                         if(nf)
    722                                 {
    723                                 bind(nf,0,f,e);
    724                                 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
    725                                 horizon.cf=nf;
    726                                 ++horizon.nf;
    727                                 return(true);
    728                                 }
    729                         }
    730                         else
    731                         {
    732                         const U e2=i2m3[e];
    733                         f->pass         =       (U1)pass;
    734                         if(     expand(pass,w,f->f[e1],f->e[e1],horizon)&&
    735                                 expand(pass,w,f->f[e2],f->e[e2],horizon))
    736                                 {
    737                                 remove(m_hull,f);
    738                                 append(m_stock,f);
    739                                 return(true);
    740                                 }
    741                         }
    742                 }
    743         return(false);
    744         }
    745 
    746 };
    747 
    748 //
    749 static void     Initialize(     const btConvexShape* shape0,const btTransform& wtrs0,
    750                                                 const btConvexShape* shape1,const btTransform& wtrs1,
    751                                                 btGjkEpaSolver2::sResults& results,
    752                                                 tShape& shape,
    753                                                 bool withmargins)
    754 {
    755 /* Results              */
    756 results.witnesses[0]    =
    757 results.witnesses[1]    =       btVector3(0,0,0);
    758 results.status                  =       btGjkEpaSolver2::sResults::Separated;
    759 /* Shape                */
    760 shape.m_shapes[0]               =       shape0;
    761 shape.m_shapes[1]               =       shape1;
    762 shape.m_toshape1                =       wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
    763 shape.m_toshape0                =       wtrs0.inverseTimes(wtrs1);
    764 shape.EnableMargin(withmargins);
    765 }
    766766
    767767}
    … …  
    776776int                     btGjkEpaSolver2::StackSizeRequirement()
    777777{
    778 return(sizeof(GJK)+sizeof(EPA));
     778        return(sizeof(GJK)+sizeof(EPA));
    779779}
    780780
    781781//
    782782bool            btGjkEpaSolver2::Distance(      const btConvexShape*    shape0,
    783                                                                                 const btTransform&              wtrs0,
    784                                                                                 const btConvexShape*    shape1,
    785                                                                                 const btTransform&              wtrs1,
    786                                                                                 const btVector3&                guess,
    787                                                                                 sResults&                               results)
     783                                                                          const btTransform&            wtrs0,
     784                                                                          const btConvexShape*  shape1,
     785                                                                          const btTransform&            wtrs1,
     786                                                                          const btVector3&              guess,
     787                                                                          sResults&                             results)
    788788{
    789 tShape                  shape;
    790 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
    791 GJK                             gjk;
    792 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
    793 if(gjk_status==GJK::eStatus::Valid)
     789        tShape                  shape;
     790        Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
     791        GJK                             gjk;
     792        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
     793        if(gjk_status==GJK::eStatus::Valid)
    794794        {
    795         btVector3       w0=btVector3(0,0,0);
    796         btVector3       w1=btVector3(0,0,0);
    797         for(U i=0;i<gjk.m_simplex->rank;++i)
    798                 {
    799                 const btScalar  p=gjk.m_simplex->p[i];
    800                 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
    801                 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
     795                btVector3       w0=btVector3(0,0,0);
     796                btVector3       w1=btVector3(0,0,0);
     797                for(U i=0;i<gjk.m_simplex->rank;++i)
     798                {
     799                        const btScalar  p=gjk.m_simplex->p[i];
     800                        w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
     801                        w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
    802802                }
    803         results.witnesses[0]    =       wtrs0*w0;
    804         results.witnesses[1]    =       wtrs0*w1;
    805         results.normal                  =       w0-w1;
    806         results.distance                =       results.normal.length();
    807         results.normal                  /=      results.distance>GJK_MIN_DISTANCE?results.distance:1;
    808         return(true);
     803                results.witnesses[0]    =       wtrs0*w0;
     804                results.witnesses[1]    =       wtrs0*w1;
     805                results.normal                  =       w0-w1;
     806                results.distance                =       results.normal.length();
     807                results.normal                  /=      results.distance>GJK_MIN_DISTANCE?results.distance:1;
     808                return(true);
    809809        }
    810810        else
    811811        {
    812         results.status  =       gjk_status==GJK::eStatus::Inside?
    813                                                         sResults::Penetrating   :
    814                                                         sResults::GJK_Failed    ;
    815         return(false);
     812                results.status  =       gjk_status==GJK::eStatus::Inside?
     813                        sResults::Penetrating   :
     814                sResults::GJK_Failed    ;
     815                return(false);
    816816        }
    817817}
    … …  
    819819//
    820820bool    btGjkEpaSolver2::Penetration(   const btConvexShape*    shape0,
    821                                                                                 const btTransform&              wtrs0,
    822                                                                                 const btConvexShape*    shape1,
    823                                                                                 const btTransform&              wtrs1,
    824                                                                                 const btVector3&                guess,
    825                                                                                 sResults&                               results,
    826                                                                                 bool                                    usemargins)
     821                                                                         const btTransform&             wtrs0,
     822                                                                         const btConvexShape*   shape1,
     823                                                                         const btTransform&             wtrs1,
     824                                                                         const btVector3&               guess,
     825                                                                         sResults&                              results,
     826                                                                         bool                                   usemargins)
    827827{
    828 tShape                  shape;
    829 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
    830 GJK                             gjk;   
    831 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
    832 switch(gjk_status)
     828        tShape                  shape;
     829        Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
     830        GJK                             gjk;   
     831        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
     832        switch(gjk_status)
    833833        {
    834834        case    GJK::eStatus::Inside:
    835835                {
    836                 EPA                             epa;
    837                 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
    838                 if(epa_status!=EPA::eStatus::Failed)
    839                         {
    840                         btVector3       w0=btVector3(0,0,0);
    841                         for(U i=0;i<epa.m_result.rank;++i)
    842                                 {
    843                                 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
     836                        EPA                             epa;
     837                        EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
     838                        if(epa_status!=EPA::eStatus::Failed)
     839                        {
     840                                btVector3       w0=btVector3(0,0,0);
     841                                for(U i=0;i<epa.m_result.rank;++i)
     842                                {
     843                                        w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
    844844                                }
    845                         results.status                  =       sResults::Penetrating;
    846                         results.witnesses[0]    =       wtrs0*w0;
    847                         results.witnesses[1]    =       wtrs0*(w0-epa.m_normal*epa.m_depth);
    848                         results.normal                  =       -epa.m_normal;
    849                         results.distance                =       -epa.m_depth;
    850                         return(true);
     845                                results.status                  =       sResults::Penetrating;
     846                                results.witnesses[0]    =       wtrs0*w0;
     847                                results.witnesses[1]    =       wtrs0*(w0-epa.m_normal*epa.m_depth);
     848                                results.normal                  =       -epa.m_normal;
     849                                results.distance                =       -epa.m_depth;
     850                                return(true);
    851851                        } else results.status=sResults::EPA_Failed;
    852852                }
    853         break;
     853                break;
    854854        case    GJK::eStatus::Failed:
    855         results.status=sResults::GJK_Failed;
    856         break;
     855                results.status=sResults::GJK_Failed;
     856                break;
    857857        }
    858 return(false);
     858        return(false);
    859859}
    860860
    … …  
    866866                                                                                        sResults& results)
    867867{
    868 tShape                  shape;
    869 btSphereShape   shape1(margin);
    870 btTransform             wtrs1(btQuaternion(0,0,0,1),position);
    871 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
    872 GJK                             gjk;   
    873 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
    874 if(gjk_status==GJK::eStatus::Valid)
     868        tShape                  shape;
     869        btSphereShape   shape1(margin);
     870        btTransform             wtrs1(btQuaternion(0,0,0,1),position);
     871        Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
     872        GJK                             gjk;   
     873        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
     874        if(gjk_status==GJK::eStatus::Valid)
    875875        {
    876         btVector3       w0=btVector3(0,0,0);
    877         btVector3       w1=btVector3(0,0,0);
    878         for(U i=0;i<gjk.m_simplex->rank;++i)
    879                 {
    880                 const btScalar  p=gjk.m_simplex->p[i];
    881                 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
    882                 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
     876                btVector3       w0=btVector3(0,0,0);
     877                btVector3       w1=btVector3(0,0,0);
     878                for(U i=0;i<gjk.m_simplex->rank;++i)
     879                {
     880                        const btScalar  p=gjk.m_simplex->p[i];
     881                        w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
     882                        w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
    883883                }
    884         results.witnesses[0]    =       wtrs0*w0;
    885         results.witnesses[1]    =       wtrs0*w1;
    886         const btVector3 delta=  results.witnesses[1]-
    887                                                         results.witnesses[0];
    888         const btScalar  margin= shape0->getMargin()+
    889                                                         shape1.getMargin();
    890         const btScalar  length= delta.length();
    891         results.normal                  =       delta/length;
    892         results.witnesses[0]    +=      results.normal*margin;
    893         return(length-margin);
     884                results.witnesses[0]    =       wtrs0*w0;
     885                results.witnesses[1]    =       wtrs0*w1;
     886                const btVector3 delta=  results.witnesses[1]-
     887                        results.witnesses[0];
     888                const btScalar  margin= shape0->getMarginNonVirtual()+
     889                        shape1.getMarginNonVirtual();
     890                const btScalar  length= delta.length();
     891                results.normal                  =       delta/length;
     892                results.witnesses[0]    +=      results.normal*margin;
     893                return(length-margin);
    894894        }
    895895        else
    896896        {
    897         if(gjk_status==GJK::eStatus::Inside)
    898                 {
    899                 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
    900                         {
    901                         const btVector3 delta=  results.witnesses[0]-
    902                                                                         results.witnesses[1];
    903                         const btScalar  length= delta.length();
    904                         if (length >= SIMD_EPSILON)
    905                                 results.normal  =       delta/length;                   
    906                         return(-length);
     897                if(gjk_status==GJK::eStatus::Inside)
     898                {
     899                        if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
     900                        {
     901                                const btVector3 delta=  results.witnesses[0]-
     902                                        results.witnesses[1];
     903                                const btScalar  length= delta.length();
     904                                if (length >= SIMD_EPSILON)
     905                                        results.normal  =       delta/length;                   
     906                                return(-length);
    907907                        }
    908908                }       
    909909        }
    910 return(SIMD_INFINITY);
     910        return(SIMD_INFINITY);
    911911}
    912912
    … …  
    919919                                                                                sResults&                               results)
    920920{
    921 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
    922         return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
     921        if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
     922                return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
    923923        else
    924         return(true);
     924                return(true);
    925925}
    926926

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

    r2192 r2430  
    1919#include "btGjkEpaPenetrationDepthSolver.h"
    2020
    21 #ifndef __SPU__
    22 //#define USE_ORIGINAL_GJK 1
    23 #endif
    24 
    25 #ifdef USE_ORIGINAL_GJK
    26 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
    27 #endif
    2821
    2922#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
    … …  
    3225                                                                                          const btConvexShape* pConvexA, const btConvexShape* pConvexB,
    3326                                                                                          const btTransform& transformA, const btTransform& transformB,
    34                                                                                           btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
     27                                                                                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
    3528                                                                                          class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
    3629{
    … …  
    4235        const btScalar                          radialmargin(btScalar(0.));
    4336       
    44 //#define USE_ORIGINAL_GJK 1
    45 #ifdef USE_ORIGINAL_GJK
    46         btGjkEpaSolver::sResults        results;
    47         if(btGjkEpaSolver::Collide(     pConvexA,transformA,
    48                                                                 pConvexB,transformB,
    49                                                                 radialmargin,stackAlloc,results))
    50 #else
    5137        btVector3       guessVector(transformA.getOrigin()-transformB.getOrigin());
    5238        btGjkEpaSolver2::sResults       results;
    … …  
    5541                                                                guessVector,results))
    5642       
    57 #endif
    5843                {
    5944        //      debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h

    r2192 r2430  
    2929                                                                          const btConvexShape* pConvexA, const btConvexShape* pConvexB,
    3030                                                                          const btTransform& transformA, const btTransform& transformB,
    31                                                                           btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
     31                                                                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
    3232                                                                          class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
    3333

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

    r2192 r2430  
    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__
    … …  
    5454void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
    5555{
     56        m_cachedSeparatingDistance = 0.f;
     57
    5658        btScalar distance=btScalar(0.);
    5759        btVector3       normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
    … …  
    6365        localTransB.getOrigin() -= positionOffset;
    6466
    65 
     67#ifdef __SPU__
    6668        btScalar marginA = m_minkowskiA->getMarginNonVirtual();
    6769        btScalar marginB = m_minkowskiB->getMarginNonVirtual();
    68 
     70#else
     71        btScalar marginA = m_minkowskiA->getMargin();
     72        btScalar marginB = m_minkowskiB->getMargin();
    6973#ifdef TEST_NON_VIRTUAL
    70         btScalar marginAv = m_minkowskiA->getMargin();
    71         btScalar marginBv = m_minkowskiB->getMargin();
    72 
     74        btScalar marginAv = m_minkowskiA->getMarginNonVirtual();
     75        btScalar marginBv = m_minkowskiB->getMarginNonVirtual();
    7376        btAssert(marginA == marginAv);
    7477        btAssert(marginB == marginBv);
    7578#endif //TEST_NON_VIRTUAL
     79#endif
     80       
     81
    7682
    7783        gNumGjkChecks++;
    … …  
    118124                        btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
    119125
     126#ifdef __SPU__
     127                        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
     128                        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
     129#else
     130                        btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
     131                        btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
    120132#ifdef TEST_NON_VIRTUAL
    121133                        btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
    122134                        btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
    123 #endif
    124                         btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
    125                         btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
    126 #ifdef TEST_NON_VIRTUAL
    127135                        btAssert((pInAv-pInA).length() < 0.0001);
    128136                        btAssert((qInBv-qInB).length() < 0.0001);
    129137#endif //
    130 
    131                         btPoint3  pWorld = localTransA(pInA);   
    132                         btPoint3  qWorld = localTransB(qInB);
     138#endif //__SPU__
     139
     140                        btVector3  pWorld = localTransA(pInA); 
     141                        btVector3  qWorld = localTransB(qInB);
    133142
    134143#ifdef DEBUG_SPU_COLLISION_DETECTION
    … …  
    332341                spu_printf("output 1\n");
    333342#endif
     343                m_cachedSeparatingAxis = normalInB;
     344                m_cachedSeparatingDistance = distance;
     345
    334346                output.addContactPoint(
    335347                        normalInB,

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

    r2192 r2430  
    2121
    2222#include "btDiscreteCollisionDetectorInterface.h"
    23 #include "LinearMath/btPoint3.h"
    2423#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
    2524
    … …  
    3938        const btConvexShape* m_minkowskiB;
    4039        bool            m_ignoreMargin;
     40        btScalar        m_cachedSeparatingDistance;
    4141       
    4242
    … …  
    6969        }
    7070
     71        const btVector3& getCachedSeparatingAxis() const
     72        {
     73                return m_cachedSeparatingAxis;
     74        }
     75        btScalar        getCachedSeparatingDistance() const
     76        {
     77                return m_cachedSeparatingDistance;
     78        }
     79
    7180        void    setPenetrationDepthSolver(btConvexPenetrationDepthSolver*       penetrationDepthSolver)
    7281        {

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

    r2192 r2430  
    3333                                m_appliedImpulse(0.f),
    3434                                m_lateralFrictionInitialized(false),
     35                                m_appliedImpulseLateral1(0.f),
     36                                m_appliedImpulseLateral2(0.f),
    3537                                m_lifeTime(0)
    3638                        {

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

    r2192 r2430  
    7171                                                                                                   const btConvexShape* convexA,const btConvexShape* convexB,
    7272                                                                                                   const btTransform& transA,const btTransform& transB,
    73                                                                                                    btVector3& v, btPoint3& pa, btPoint3& pb,
     73                                                                                                   btVector3& v, btVector3& pa, btVector3& pb,
    7474                                                                                                   class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
    7575                                                                                                   )

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h

    r2192 r2430  
    2828        const btConvexShape* convexA,const btConvexShape* convexB,
    2929                                const btTransform& transA,const btTransform& transB,
    30                         btVector3& v, btPoint3& pa, btPoint3& pb,
     30                        btVector3& v, btVector3& pa, btVector3& pb,
    3131                        class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
    3232                        );

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

    r2192 r2430  
    1717#include "btPersistentManifold.h"
    1818#include "LinearMath/btTransform.h"
    19 #include <assert.h>
     19
    2020
    2121btScalar                                        gContactBreakingThreshold = btScalar(0.02);
    … …  
    6767                        }
    6868                }
    69                 assert(occurance<=0);
     69                btAssert(occurance<=0);
    7070#endif //DEBUG_PERSISTENCY
    7171
    … …  
    165165int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
    166166{
    167         assert(validContactDistance(newPoint));
     167        btAssert(validContactDistance(newPoint));
    168168
    169169        int insertIndex = getNumContacts();
    … …  
    191191btScalar        btPersistentManifold::getContactBreakingThreshold() const
    192192{
    193         return gContactBreakingThreshold;
     193        return m_contactBreakingThreshold;
    194194}
    195195

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

    r2192 r2430  
    2525struct btCollisionResult;
    2626
    27 ///contact breaking and merging threshold
     27///maximum contact breaking and merging threshold
    2828extern btScalar gContactBreakingThreshold;
    2929
    … …  
    5555        int     m_cachedPoints;
    5656
     57        btScalar        m_contactBreakingThreshold;
     58
    5759       
    5860        /// sort cached points so most isolated points come first
    … …  
    6971        btPersistentManifold();
    7072
    71         btPersistentManifold(void* body0,void* body1,int bla)
    72                 : m_body0(body0),m_body1(body1),m_cachedPoints(0)
     73        btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold)
     74                : m_body0(body0),m_body1(body1),m_cachedPoints(0),
     75                m_contactBreakingThreshold(contactBreakingThreshold)
    7376        {
    74                 (void)bla;
     77               
    7578        }
    7679
    … …  
    107110        }
    108111
    109         /// todo: get this margin from the current physics / collision environment
     112        ///@todo: get this margin from the current physics / collision environment
    110113        btScalar        getContactBreakingThreshold() const;
    111114       

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h

    r2192 r2430  
    2020
    2121#include "LinearMath/btVector3.h"
    22 #include "LinearMath/btPoint3.h"
    2322
    2423#define NO_VIRTUAL_INTERFACE 1
    … …  
    3837        virtual void reset() = 0;
    3938
    40         virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0;
     39        virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0;
    4140       
    4241        virtual bool closest(btVector3& v) = 0;
    … …  
    4645        virtual bool fullSimplex() const = 0;
    4746
    48         virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0;
     47        virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0;
    4948
    5049        virtual bool inSimplex(const btVector3& w) = 0;
    … …  
    5453        virtual bool emptySimplex() const = 0;
    5554
    56         virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0;
     55        virtual void compute_points(btVector3& p1, btVector3& p2) = 0;
    5756
    5857        virtual int numVertices() const =0;

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

    r2192 r2430  
    7878
    7979        //add a vertex
    80 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
     80void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
    8181{
    8282        m_lastW = w;
    … …  
    268268
    269269        //return the current simplex
    270 int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
     270int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
    271271{
    272272        int i;
    … …  
    315315}
    316316
    317 void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
     317void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
    318318{
    319319        updateClosestVectorAndPoints();
    … …  
    326326
    327327
    328 bool    btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result)
     328bool    btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
    329329{
    330330        result.m_usedVertices.reset();
    … …  
    426426
    427427/// Test if point p and d lie on opposite sides of plane through abc
    428 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
     428int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
    429429{
    430430        btVector3 normal = (b-a).cross(c-a);
    … …  
    453453
    454454
    455 bool    btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult)
     455bool    btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
    456456{
    457457        btSubSimplexClosestResult tempResult;
    … …  
    487487        {
    488488        closestPtPointTriangle(p, a, b, c,tempResult);
    489                 btPoint3 q = tempResult.m_closestPointOnSimplex;
     489                btVector3 q = tempResult.m_closestPointOnSimplex;
    490490               
    491491        btScalar sqDist = (q - p).dot( q - p);
    … …  
    514514        {
    515515        closestPtPointTriangle(p, a, c, d,tempResult);
    516                 btPoint3 q = tempResult.m_closestPointOnSimplex;
     516                btVector3 q = tempResult.m_closestPointOnSimplex;
    517517                //convert result bitmask!
    518518
    … …  
    542542        {
    543543                closestPtPointTriangle(p, a, d, b,tempResult);
    544                 btPoint3 q = tempResult.m_closestPointOnSimplex;
     544                btVector3 q = tempResult.m_closestPointOnSimplex;
    545545                //convert result bitmask!
    546546
    … …  
    570570        {
    571571        closestPtPointTriangle(p, b, d, c,tempResult);
    572                 btPoint3 q = tempResult.m_closestPointOnSimplex;
     572                btVector3 q = tempResult.m_closestPointOnSimplex;
    573573                //convert result bitmask!
    574574        btScalar sqDist = (q - p).dot( q - p);

  • code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h

    r2192 r2430  
    5151struct  btSubSimplexClosestResult
    5252{
    53         btPoint3        m_closestPointOnSimplex;
     53        btVector3       m_closestPointOnSimplex;
    5454        //MASK for m_usedVertices
    5555        //stores the simplex vertex-usage, using the MASK,
    … …  
    9898
    9999        btVector3       m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
    100         btPoint3        m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
    101         btPoint3        m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
     100        btVector3       m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
     101        btVector3       m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
    102102
    103103       
    104104
    105         btPoint3        m_cachedP1;
    106         btPoint3        m_cachedP2;
     105        btVector3       m_cachedP1;
     106        btVector3       m_cachedP2;
    107107        btVector3       m_cachedV;
    108108        btVector3       m_lastW;
    … …  
    117117        bool    updateClosestVectorAndPoints();
    118118
    119         bool    closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult);
    120         int             pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d);
    121         bool    closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result);
     119        bool    closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult);
     120        int             pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
     121        bool    closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result);
    122122
    123123public:
    … …  
    125125         void reset();
    126126
    127          void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
     127         void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
    128128
    129129
    … …  
    137137         }
    138138
    139          int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
     139         int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
    140140
    141141         bool inSimplex(const btVector3& w);
    … …  
    145145         bool emptySimplex() const ;
    146146
    147          void compute_points(btPoint3& p1, btPoint3& p2) ;
     147         void compute_points(btVector3& p1, btVector3& p2) ;
    148148
    149149         int numVertices() const

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

    r2192 r2430  
    1 ADD_LIBRARY(LibBulletDynamics
    2 
     1SET(BulletDynamics_SRCS
    32        ConstraintSolver/btContactConstraint.cpp
    4         ConstraintSolver/btContactConstraint.h
    53        ConstraintSolver/btConeTwistConstraint.cpp
    6         ConstraintSolver/btConeTwistConstraint.h
    74        ConstraintSolver/btGeneric6DofConstraint.cpp
    8         ConstraintSolver/btGeneric6DofConstraint.h
    95        ConstraintSolver/btHingeConstraint.cpp
    10         ConstraintSolver/btHingeConstraint.h
    116        ConstraintSolver/btPoint2PointConstraint.cpp
    12         ConstraintSolver/btPoint2PointConstraint.h
    137        ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
    14         ConstraintSolver/btSequentialImpulseConstraintSolver.h
    158        ConstraintSolver/btSliderConstraint.cpp
    16         ConstraintSolver/btSliderConstraint.h
    179        ConstraintSolver/btSolve2LinearConstraint.cpp
    18         ConstraintSolver/btSolve2LinearConstraint.h
    1910        ConstraintSolver/btTypedConstraint.cpp
    20         ConstraintSolver/btTypedConstraint.h
    2111        Dynamics/Bullet-C-API.cpp
    2212        Dynamics/btDiscreteDynamicsWorld.cpp
    23         Dynamics/btDiscreteDynamicsWorld.h
    2413        Dynamics/btSimpleDynamicsWorld.cpp
    25         Dynamics/btSimpleDynamicsWorld.h
    2614        Dynamics/btRigidBody.cpp
    27         Dynamics/btRigidBody.h
    2815        Vehicle/btRaycastVehicle.cpp
    29         Vehicle/btRaycastVehicle.h
    3016        Vehicle/btWheelInfo.cpp
    31         Vehicle/btWheelInfo.h
     17        Character/btKinematicCharacterController.cpp
    3218)
     19
     20ADD_LIBRARY(BulletDynamics ${BulletDynamics_SRCS})

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

    r2192 r2430  
    2121#define CONETWISTCONSTRAINT_H
    2222
    23 #include "../../LinearMath/btVector3.h"
     23#include "LinearMath/btVector3.h"
    2424#include "btJacobianEntry.h"
    2525#include "btTypedConstraint.h"

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

    r2192 r2430  
    426426
    427427        return btScalar(0.);
    428 };
    429 
     428}
     429

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

    r2192 r2430  
    1717#define CONTACT_CONSTRAINT_H
    1818
    19 //todo: make into a proper class working with the iterative constraint solver
     19///@todo: make into a proper class working with the iterative constraint solver
    2020
    2121class btRigidBody;

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

    r2192 r2430  
    2222        SOLVER_FRICTION_SEPARATE = 2,
    2323        SOLVER_USE_WARMSTARTING = 4,
    24         SOLVER_CACHE_FRIENDLY = 8
     24        SOLVER_USE_FRICTION_WARMSTARTING = 8,
     25        SOLVER_CACHE_FRIENDLY = 16
    2526};
    2627
    … …  
    4546
    4647        int                     m_solverMode;
     48        int     m_restingContactRestitutionThreshold;
    4749
    4850
    … …  
    6971                m_linearSlop = btScalar(0.0);
    7072                m_warmstartingFactor=btScalar(0.85);
    71                 m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING;
     73                m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
     74                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
    7275        }
    7376};

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

    r2192 r2430  
    2727
    2828
    29 static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
    30 static const int kAxisA[] = { 1, 0, 0 };
    31 static const int kAxisB[] = { 2, 2, 1 };
    3229#define GENERIC_D6_DISABLE_WARMSTARTING 1
    3330
    … …  
    157154    btScalar clippedMotorImpulse;
    158155
    159     //todo: should clip against accumulated impulse
     156    ///@todo: should clip against accumulated impulse
    160157    if (unclippedMotorImpulse>0.0f)
    161158    {

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

    r2192 r2430  
    324324                                        getRigidBodyB().computeAngularImpulseDenominator(normal);
    325325                                // scale for mass and relaxation
    326                                 //todo:  expose this 0.9 factor to developer
    327326                                velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
    328327                        }

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

    r2192 r2430  
    438438
    439439
    440 void    btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
     440btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
    441441{
    442442
    … …  
    492492        solverConstraint.m_jacDiagABInv = denom;
    493493
    494 
     494        return solverConstraint;
    495495}
    496496
    … …  
    719719
    720720                                                        solverConstraint.m_friction = cp.m_combinedFriction;
    721                                                         solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
    722                                                         if (solverConstraint.m_restitution <= btScalar(0.))
     721
     722                                                       
     723                                                        if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
    723724                                                        {
    724725                                                                solverConstraint.m_restitution = 0.f;
    725                                                         };
     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                                                        }
    726734
    727735                                                       
    … …  
    761769                                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
    762770                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    763                                                                         cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
    764                                                                         cp.m_lateralFrictionDir2.normalize();//??
    765                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     771                                                                        if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     772                                                                        {
     773                                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
     774                                                                                cp.m_lateralFrictionDir2.normalize();//??
     775                                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     776                                                                                cp.m_lateralFrictionInitialized = true;
     777                                                                        }
    766778                                                                } else
    767779                                                                {
    768780                                                                        //re-calculate friction direction every frame, todo: check if this is really needed
    769                                                                        
    770781                                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
    771782                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    772783                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     784                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     785                                                                        {
     786                                                                                cp.m_lateralFrictionInitialized = true;
     787                                                                        }
    773788                                                                }
    774                                                                 cp.m_lateralFrictionInitialized = true;
    775789                                                               
    776790                                                        } else
    777791                                                        {
    778792                                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    779                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     793                                                                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     794                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    780795                                                        }
    781796
     797                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    782798                                                        {
    783                                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
    784                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    785799                                                                {
    786                                                                         frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
    787                                                                         if (rb0)
    788                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
    789                                                                         if (rb1)
    790                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
    791                                                                 } else
     800                                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
     801                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     802                                                                        {
     803                                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
     804                                                                                if (rb0)
     805                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
     806                                                                                if (rb1)
     807                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
     808                                                                        } else
     809                                                                        {
     810                                                                                frictionConstraint1.m_appliedImpulse = 0.f;
     811                                                                        }
     812                                                                }
    792813                                                                {
    793                                                                         frictionConstraint1.m_appliedImpulse = 0.f;
    794                                                                 }
    795                                                         }
    796                                                         {
    797                                                                 btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    798                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    799                                                                 {
    800                                                                         frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
    801                                                                         if (rb0)
    802                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
    803                                                                         if (rb1)
    804                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
    805                                                                 } else
    806                                                                 {
    807                                                                         frictionConstraint2.m_appliedImpulse = 0.f;
     814                                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
     815                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     816                                                                        {
     817                                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
     818                                                                                if (rb0)
     819                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
     820                                                                                if (rb1)
     821                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
     822                                                                        } else
     823                                                                        {
     824                                                                                frictionConstraint2.m_appliedImpulse = 0.f;
     825                                                                        }
    808826                                                                }
    809827                                                        }
    … …  
    833851        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    834852
    835         ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
     853        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    836854        m_orderTmpConstraintPool.resize(numConstraintPool);
    837855        m_orderFrictionConstraintPool.resize(numFrictionPool);
    … …  
    9851003                btAssert(pt);
    9861004                pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
    987                 pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    988                 pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     1005                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     1006                {
     1007                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1008                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     1009                }
    9891010
    9901011                //do a callback here?
    … …  
    12211242                                cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
    12221243                                cpd->m_friction = cp.m_combinedFriction;
    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                                 };
     1244                                if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
     1245                                {
     1246                                        cpd->m_restitution = 0.f;
     1247                                } else
     1248                                {
     1249                                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
     1250                                        if (cpd->m_restitution <= btScalar(0.))
     1251                                        {
     1252                                                cpd->m_restitution = btScalar(0.0);
     1253                                        };
     1254                                }
    12291255                               
    12301256                                //restitution and penetration work in same direction so

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

    r2192 r2430  
    2424
    2525
    26 /// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
    27 /// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
    28 /// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
    29 /// Applies impulses for combined restitution and penetration recovery and to simulate friction
     26///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
     27///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
     28///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
     29///Applies impulses for combined restitution and penetration recovery and to simulate friction
    3030class btSequentialImpulseConstraintSolver : public btConstraintSolver
    3131{
    … …  
    4242        btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
    4343        void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
    44         void    addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
     44        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
    4545
    4646        ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];

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

    r2192 r2430  
    2525
    2626
    27 ///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
     27///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
    2828ATTRIBUTE_ALIGNED16 (struct)    btSolverBody
    2929{
    3030        BT_DECLARE_ALIGNED_ALLOCATOR();
    3131       
     32        btMatrix3x3             m_worldInvInertiaTensor;
     33
    3234        btVector3               m_angularVelocity;
     35        btVector3               m_linearVelocity;
     36        btVector3               m_centerOfMassPosition;
     37        btVector3               m_pushVelocity;
     38        btVector3               m_turnVelocity;
     39
    3340        float                   m_angularFactor;
    3441        float                   m_invMass;
    3542        float                   m_friction;
    3643        btRigidBody*    m_originalBody;
    37         btVector3               m_linearVelocity;
    38         btVector3               m_centerOfMassPosition;
    39        
    40         btVector3               m_pushVelocity;
    41         btVector3               m_turnVelocity;
    4244       
    4345       

  • code/branches/physics/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp

    r2192 r2430  
    3939#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
    4040#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
    41 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
     41#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
    4242#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
    4343#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
    … …  
    256256        btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
    257257        btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
    258         convexHullShape->addPoint(btPoint3(x,y,z));
     258        convexHullShape->addPoint(btVector3(x,y,z));
    259259
    260260}

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

    r2192 r2430  
    5252#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
    5353#include "BulletDynamics/Vehicle/btWheelInfo.h"
     54//character
     55#include "BulletDynamics/Character/btCharacterControllerInterface.h"
     56
    5457#include "LinearMath/btIDebugDraw.h"
    5558#include "LinearMath/btQuickprof.h"
    … …  
    126129void    btDiscreteDynamicsWorld::debugDrawWorld()
    127130{
     131        BT_PROFILE("debugDrawWorld");
    128132
    129133        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
    … …  
    151155                int i;
    152156
    153                 //todo: iterate over awake simulation islands!
    154157                for (  i=0;i<m_collisionObjects.size();i++)
    155158                {
    … …  
    180183                        if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
    181184                        {
    182                                 btPoint3 minAabb,maxAabb;
     185                                btVector3 minAabb,maxAabb;
    183186                                btVector3 colorvec(1,0,0);
    184187                                colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
    … …  
    221224void    btDiscreteDynamicsWorld::clearForces()
    222225{
    223         //todo: iterate over awake simulation islands!
     226        ///@todo: iterate over awake simulation islands!
    224227        for ( int i=0;i<m_collisionObjects.size();i++)
    225228        {
    … …  
    237240void    btDiscreteDynamicsWorld::applyGravity()
    238241{
    239         //todo: iterate over awake simulation islands!
     242        ///@todo: iterate over awake simulation islands!
    240243        for ( int i=0;i<m_collisionObjects.size();i++)
    241244        {
    … …  
    251254
    252255
     256void    btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
     257{
     258        btAssert(body);
     259
     260        if (body->getMotionState() && !body->isStaticOrKinematicObject())
     261        {
     262                //we need to call the update at least once, even for sleeping objects
     263                //otherwise the 'graphics' transform never updates properly
     264                ///@todo: add 'dirty' flag
     265                //if (body->getActivationState() != ISLAND_SLEEPING)
     266                {
     267                        btTransform interpolatedTransform;
     268                        btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
     269                                body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
     270                        body->getMotionState()->setWorldTransform(interpolatedTransform);
     271                }
     272        }
     273}
     274
    253275
    254276void    btDiscreteDynamicsWorld::synchronizeMotionStates()
    255277{
     278        BT_PROFILE("synchronizeMotionStates");
    256279        {
    257280                //todo: iterate over awake simulation islands!
    … …  
    261284                       
    262285                        btRigidBody* body = btRigidBody::upcast(colObj);
    263                         if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
    264                         {
    265                                 //we need to call the update at least once, even for sleeping objects
    266                                 //otherwise the 'graphics' transform never updates properly
    267                                 //so todo: add 'dirty' flag
    268                                 //if (body->getActivationState() != ISLAND_SLEEPING)
    269                                 {
    270                                         btTransform interpolatedTransform;
    271                                         btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
    272                                                 body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
    273                                         body->getMotionState()->setWorldTransform(interpolatedTransform);
    274                                 }
    275                         }
     286                        if (body)
     287                                synchronizeSingleMotionState(body);
    276288                }
    277289        }
    … …  
    393405        ///update vehicle simulation
    394406        updateVehicles(timeStep);
    395 
     407       
     408        updateCharacters(timeStep);
    396409
    397410        updateActivationState( timeStep );
    … …  
    469482}
    470483
     484void    btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
     485{
     486        BT_PROFILE("updateCharacters");
     487       
     488        for ( int i=0;i<m_characters.size();i++)
     489        {
     490                btCharacterControllerInterface* character = m_characters[i];
     491                character->preStep (this);
     492                character->playerStep (this,timeStep);
     493        }
     494}
     495
     496       
     497       
    471498void    btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
    472499{
    … …  
    532559        m_vehicles.remove(vehicle);
    533560}
     561
     562void    btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
     563{
     564        m_characters.push_back(character);
     565}
     566
     567void    btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
     568{
     569        m_characters.remove(character);
     570}
     571
    534572
    535573SIMD_FORCE_INLINE       int     btGetConstraintIslandId(const btTypedConstraint* lhs)
    … …  
    665703       
    666704        /// solve all the constraints for this island
    667         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
     705        m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
    668706
    669707        m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
    … …  
    730768        {
    731769                if (convexResult.m_hitCollisionObject == m_me)
    732                         return 1.0;
     770                        return 1.0f;
     771
     772                //ignore result if there is no contact response
     773                if(!convexResult.m_hitCollisionObject->hasContactResponse())
     774                        return 1.0f;
    733775
    734776                btVector3 linVelA,linVelB;
    … …  
    751793
    752794                ///don't do CCD when the collision filters are not matching
    753                 if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
     795                if (!ClosestConvexResultCallback::needsCollision(proxy0))
    754796                        return false;
    755797
    … …  
    808850                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    809851                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
     852
     853                                                sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
     854                                                sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
     855
    810856                                                convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
    811857                                                if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
    … …  
    827873
    828874
     875
     876
    829877void    btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
    830878{
    … …  
    838886                        if (!body->isStaticOrKinematicObject())
    839887                        {
    840                                 if (body->isActive())
    841                                 {
    842                                         body->integrateVelocities( timeStep);
    843                                         //damping
    844                                         body->applyDamping(timeStep);
    845 
    846                                         body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
    847                                 }
     888                               
     889                                body->integrateVelocities( timeStep);
     890                                //damping
     891                                body->applyDamping(timeStep);
     892
     893                                body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
    848894                        }
    849895                }
    … …  
    10971143                                        btConcaveShape* concaveMesh = (btConcaveShape*) shape;
    10981144                                       
    1099                                         //todo pass camera, for some culling
     1145                                        ///@todo pass camera, for some culling? no -> we are not a graphics lib
    11001146                                        btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    11011147                                        btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    … …  
    11261172                                        for (i=0;i<polyshape->getNumEdges();i++)
    11271173                                        {
    1128                                                 btPoint3 a,b;
     1174                                                btVector3 a,b;
    11291175                                                polyshape->getEdge(i,a,b);
    11301176                                                btVector3 wa = worldTransform * a;

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

    r2192 r2430  
    2727
    2828class btRaycastVehicle;
     29class btCharacterControllerInterface;
    2930class btIDebugDraw;
    3031#include "LinearMath/btAlignedObjectArray.h"
    … …  
    4344        btAlignedObjectArray<btTypedConstraint*> m_constraints;
    4445
    45 
    4646        btVector3       m_gravity;
    4747
    … …  
    5555       
    5656        btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
     57       
     58        btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
     59       
    5760
    5861        int     m_profileTimings;
    … …  
    6265        virtual void    integrateTransforms(btScalar timeStep);
    6366               
    64         void    calculateSimulationIslands();
     67        virtual void    calculateSimulationIslands();
    6568
    66         void    solveConstraints(btContactSolverInfo& solverInfo);
     69        virtual void    solveConstraints(btContactSolverInfo& solverInfo);
    6770       
    6871        void    updateActivationState(btScalar timeStep);
    … …  
    7073        void    updateVehicles(btScalar timeStep);
    7174
     75        void    updateCharacters(btScalar timeStep);
     76
    7277        void    startProfiling(btScalar timeStep);
    7378
    7479        virtual void    internalSingleStepSimulation( btScalar timeStep);
    7580
    76         void    synchronizeMotionStates();
    7781
    78         void    saveKinematicState(btScalar timeStep);
     82        virtual void    saveKinematicState(btScalar timeStep);
    7983
    8084        void    debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
     85
    8186
    8287public:
    … …  
    9297
    9398
    94         void    addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
     99        virtual void    synchronizeMotionStates();
    95100
    96         void    removeConstraint(btTypedConstraint* constraint);
     101        ///this can be useful to synchronize a single rigid body -> graphics object
     102        void    synchronizeSingleMotionState(btRigidBody* body);
    97103
    98         void    addVehicle(btRaycastVehicle* vehicle);
     104        virtual void    addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
    99105
    100         void    removeVehicle(btRaycastVehicle* vehicle);
     106        virtual void    removeConstraint(btTypedConstraint* constraint);
     107
     108        virtual void    addVehicle(btRaycastVehicle* vehicle);
     109
     110        virtual void    removeVehicle(btRaycastVehicle* vehicle);
     111       
     112        virtual void    addCharacter(btCharacterControllerInterface* character);
     113
     114        virtual void    removeCharacter(btCharacterControllerInterface* character);
     115               
    101116
    102117        btSimulationIslandManager*      getSimulationIslandManager()
    … …  
    114129                return this;
    115130        }
    116 
    117131
    118132        virtual void    setGravity(const btVector3& gravity);
    … …  
    153167        virtual void    setNumTasks(int numTasks)
    154168        {
     169        (void) numTasks;
    155170        }
    156171

  • code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

    r2192 r2430  
    2424class btConstraintSolver;
    2525class btDynamicsWorld;
     26class btCharacterControllerInterface;
    2627
    2728/// Type for the callback for each tick
    … …  
    7677                virtual void    removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
    7778
     79                virtual void    addCharacter(btCharacterControllerInterface* character) {(void)character;}
     80
     81                virtual void    removeCharacter(btCharacterControllerInterface* character) {(void)character;}
     82
     83
    7884                //once a rigidbody is added to the dynamics world, it will get this gravity assigned
    7985                //existing rigidbodies in the world get gravity assigned too, during this method
    8086                virtual void    setGravity(const btVector3& gravity) = 0;
    8187                virtual btVector3 getGravity () const = 0;
     88
     89                virtual void    synchronizeMotionStates() = 0;
    8290
    8391                virtual void    addRigidBody(btRigidBody* body) = 0;

  • code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.h

    r2192 r2430  
    1818
    1919#include "LinearMath/btAlignedObjectArray.h"
    20 #include "LinearMath/btPoint3.h"
    2120#include "LinearMath/btTransform.h"
    2221#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
    … …  
    3231
    3332
    34 ///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
     33///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
    3534///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
    3635///There are 3 types of rigid bodies:
    … …  
    7675
    7776
    78         ///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
     77        ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
    7978        ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
    8079        ///You can use the motion state to synchronize the world transform between physics and graphics objects.
    … …  
    304303        void updateInertiaTensor();   
    305304       
    306         const btPoint3&     getCenterOfMassPosition() const {
     305        const btVector3&     getCenterOfMassPosition() const {
    307306                return m_worldTransform.getOrigin();
    308307        }
    … …  
    322321        inline void setLinearVelocity(const btVector3& lin_vel)
    323322        {
    324                 assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
    325323                m_linearVelocity = lin_vel;
    326324        }
    327325
    328         inline void setAngularVelocity(const btVector3& ang_vel) {
    329                 assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
    330                 {
    331                         m_angularVelocity = ang_vel;
    332                 }
     326        inline void setAngularVelocity(const btVector3& ang_vel)
     327        {
     328                m_angularVelocity = ang_vel;
    333329        }
    334330
    … …  
    354350
    355351       
    356         SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
     352        SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
    357353        {
    358354                btVector3 r0 = pos - getCenterOfMassPosition();

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

    r2192 r2430  
    9898void    btSimpleDynamicsWorld::clearForces()
    9999{
    100         //todo: iterate over awake simulation islands!
     100        ///@todo: iterate over awake simulation islands!
    101101        for ( int i=0;i<m_collisionObjects.size();i++)
    102102        {
    … …  
    157157                        if (body->isActive() && (!body->isStaticObject()))
    158158                        {
    159                                 btPoint3 minAabb,maxAabb;
     159                                btVector3 minAabb,maxAabb;
    160160                                colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
    161161                                btBroadphaseInterface* bp = getBroadphase();
    … …  
    211211void    btSimpleDynamicsWorld::synchronizeMotionStates()
    212212{
    213         //todo: iterate over awake simulation islands!
     213        ///@todo: iterate over awake simulation islands!
    214214        for ( int i=0;i<m_collisionObjects.size();i++)
    215215        {

  • code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

    r2192 r2430  
    2323class btConstraintSolver;
    2424
    25 ///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
     25///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
    2626///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished).
    2727class btSimpleDynamicsWorld : public btDynamicsWorld
    … …  
    6161        virtual void    updateAabbs();
    6262
    63         void    synchronizeMotionStates();
     63        virtual void    synchronizeMotionStates();
    6464
    6565        virtual void    setConstraintSolver(btConstraintSolver* solver);

  • code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

    r2192 r2430  
    189189                wheel.m_raycastInfo.m_isInContact = true;
    190190               
    191                 wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!;
     191                wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!;
    192192                //wheel.m_raycastInfo.m_groundObject = object;
    193193
    … …  
    692692                                        btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
    693693
    694                                         rel_pos[2] *= wheelInfo.m_rollInfluence;
     694                                        rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence;
    695695                                        m_chassisBody->applyImpulse(sideImp,rel_pos);
    696696

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

    r2192 r2430  
    1 ADD_SUBDIRECTORY(BulletSoftBody)
    21ADD_SUBDIRECTORY(BulletCollision)
    32ADD_SUBDIRECTORY(BulletDynamics)

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

    r2192 r2430  
    44Todo: update changelog from April - July 2008
    55See http://code.google.com/p/bullet/source/list for more complete log in Subversion
     6
     72008 November 12
     8        - Add compound shape export to BulletColladaConverter
     9        Thanks to JamesH for the report: http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=12&t=2840
     10        - Fix compiler build for Visual Studio 6
     11        Thanks to JoF for the report: http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2841
     12
     132008 November 11
     14        - Add CProfileManager::dumpAll() to dump performance statistics to console using printf.
     15        - Add support for interaction between btSoftBody and btCollisionObject/btGhostObject
     16
     172008 November 8
     18        - Fix PosixThreadSupport
     19        - Add improved btHeightfieldTerrainShape support and new Demos/TerrainDemo
     20        Thanks to tomva, http://code.google.com/p/bullet/issues/detail?id=63&can=1
     21        - Moved kinematic character controller from Demos/CharacterDemo into src/BulletDynamics/Character/btKinematicCharacterController.cpp
     22
     232008 November 6
     24        - reduced default memory pool allocation from 40Mb to 3Mb. This should be more suitable for all platforms, including iPhone
     25        - improved CUDA broadphase
     26        - IBM Cell SDK 3.x support, fix ibmsdk Makefiles
     27        - improved CMake support with 'install' and 'framework option
     28       
     292008 November 4
     30        - add btAxisSweep::resetPool to avoid non-determinism due to shuffled linked list
     31        Thanks to Ole for the contribution,
     32
     332008 October 30
     34        - disabled btTriangleMesh duplicate search by default, it is extremely slow
     35        - added Extras/IFF binary chunk serialization library as preparation for in-game native platform serialization (planned COLLADA DOM -> IFF converter)
     36
     372008 October 20
     38        - added SCE Physics Effects box-box collision detection for SPU/BulletMultiThreaded version
     39        See Bullet/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp
     40        Thanks to Sony Computer Entertainment Japan, SCEI for the contribution
     41
     422008 October 17
     43        - Added btGhostObject support, this helps character controller, explosions, triggers and other local spatial queries
     44
     452008 October 10
     46        - Moved aabb to btBroadphaseProxy, improves rayTest dramatically. Further raytest improvements using the broadphase acceleration structures are planned
     47        - Moved BulletMultiThreaded from Extras to /src/BulletMultiThreaded for better integration
    648
    749

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

    r2192 r2430  
    1 ADD_LIBRARY(LibLinearMath
    2                 btAlignedObjectArray.h
    3                 btList.h
    4                 btPoolAllocator.h
    5                 btRandom.h
    6                 btVector3.h
    7                 btDefaultMotionState.h
    8                 btMatrix3x3.h
    9                 btQuadWord.h
    10                 btHashMap.h
    11                 btScalar.h
    12                 btAabbUtil2.h
    13                 btConvexHull.h
     1SET(LinearMath_SRCS
    142                btConvexHull.cpp
    15                 btMinMax.h
    16                 btQuaternion.h
    17                 btStackAlloc.h
    18                 btGeometryUtil.h
    19                 btMotionState.h
    20                 btTransform.h
    21                 btAlignedAllocator.h
    22                 btIDebugDraw.h
    23                 btPoint3.h
    24                 btQuickprof.h
    25                 btTransformUtil.h
    263                btQuickprof.cpp
    274                btGeometryUtil.cpp
    … …  
    296)
    307
     8ADD_LIBRARY(LinearMath ${LinearMath_SRCS})

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

    r2192 r2430  
    2222#include "btMinMax.h"
    2323
     24
     25
    2426SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
    2527                                                                   btVector3& aabbMax,
    … …  
    3133}
    3234
     35/// conservative test for overlap between two aabbs
     36SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
     37                                                                const btVector3 &point)
     38{
     39        bool overlap = true;
     40        overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
     41        overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
     42        overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
     43        return overlap;
     44}
     45
    3346
    3447/// conservative test for overlap between two aabbs
    … …  
    3750{
    3851        bool overlap = true;
    39         overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
    40         overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
    41         overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
     52        overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
     53        overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
     54        overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
    4255        return overlap;
    4356}
    … …  
    7285                   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
    7386}
     87
    7488
    7589
    … …  
    8397{
    8498        btScalar tmax, tymin, tymax, tzmin, tzmax;
    85         tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0];
    86         tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0];
    87         tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1];
    88         tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1];
     99        tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
     100        tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
     101        tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
     102        tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
    89103
    90104        if ( (tmin > tymax) || (tymin > tmax) )
    … …  
    97111                tmax = tymax;
    98112
    99         tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2];
    100         tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2];
     113        tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
     114        tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
    101115
    102116        if ( (tmin > tzmax) || (tzmin > tmax) )
    … …  
    197211}
    198212
     213#define USE_BANCHLESS 1
     214#ifdef USE_BANCHLESS
     215        //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
     216        SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
     217        {               
     218                return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
     219                        & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
     220                        & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
     221                        1, 0));
     222        }
     223#else
     224        SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
     225        {
     226                bool overlap = true;
     227                overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
     228                overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
     229                overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
     230                return overlap;
     231        }
     232#endif //USE_BANCHLESS
    199233
    200234#endif

  • code/branches/physics/src/bullet/LinearMath/btConvexHull.cpp

    r2192 r2430  
    343343}
    344344
    345 class Tri;
    346 
    347 
    348 
    349 class Tri : public int3
     345class btHullTriangle;
     346
     347
     348
     349class btHullTriangle : public int3
    350350{
    351351public:
    … …  
    354354        int vmax;
    355355        btScalar rise;
    356         Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
     356        btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
    357357        {
    358358                vmax=-1;
    359359                rise = btScalar(0.0);
    360360        }
    361         ~Tri()
     361        ~btHullTriangle()
    362362        {
    363363        }
    … …  
    366366
    367367
    368 int &Tri::neib(int a,int b)
     368int &btHullTriangle::neib(int a,int b)
    369369{
    370370        static int er=-1;
    … …  
    380380        return er;
    381381}
    382 void HullLibrary::b2bfix(Tri* s,Tri*t)
     382void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
    383383{
    384384        int i;
    … …  
    396396}
    397397
    398 void HullLibrary::removeb2b(Tri* s,Tri*t)
     398void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
    399399{
    400400        b2bfix(s,t);
    … …  
    404404}
    405405
    406 void HullLibrary::checkit(Tri *t)
     406void HullLibrary::checkit(btHullTriangle *t)
    407407{
    408408        (void)t;
    … …  
    428428}
    429429
    430 Tri*    HullLibrary::allocateTriangle(int a,int b,int c)
    431 {
    432         void* mem = btAlignedAlloc(sizeof(Tri),16);
    433         Tri* tr = new (mem)Tri(a,b,c);
     430btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c)
     431{
     432        void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
     433        btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
    434434        tr->id = m_tris.size();
    435435        m_tris.push_back(tr);
    … …  
    438438}
    439439
    440 void    HullLibrary::deAllocateTriangle(Tri* tri)
     440void    HullLibrary::deAllocateTriangle(btHullTriangle* tri)
    441441{
    442442        btAssert(m_tris[tri->id]==tri);
    443443        m_tris[tri->id]=NULL;
    444         tri->~Tri();
     444        tri->~btHullTriangle();
    445445        btAlignedFree(tri);
    446446}
    447447
    448448
    449 void HullLibrary::extrude(Tri *t0,int v)
     449void HullLibrary::extrude(btHullTriangle *t0,int v)
    450450{
    451451        int3 t= *t0;
    452452        int n = m_tris.size();
    453         Tri* ta = allocateTriangle(v,t[1],t[2]);
     453        btHullTriangle* ta = allocateTriangle(v,t[1],t[2]);
    454454        ta->n = int3(t0->n[0],n+1,n+2);
    455455        m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
    456         Tri* tb = allocateTriangle(v,t[2],t[0]);
     456        btHullTriangle* tb = allocateTriangle(v,t[2],t[0]);
    457457        tb->n = int3(t0->n[1],n+2,n+0);
    458458        m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
    459         Tri* tc = allocateTriangle(v,t[0],t[1]);
     459        btHullTriangle* tc = allocateTriangle(v,t[0],t[1]);
    460460        tc->n = int3(t0->n[2],n+0,n+1);
    461461        m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
    … …  
    470470}
    471471
    472 Tri* HullLibrary::extrudable(btScalar epsilon)
     472btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
    473473{
    474474        int i;
    475         Tri *t=NULL;
     475        btHullTriangle *t=NULL;
    476476        for(i=0;i<m_tris.size();i++)
    477477        {
    … …  
    551551
    552552        btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0);  // a valid interior point
    553         Tri *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
    554         Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
    555         Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
    556         Tri *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
     553        btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
     554        btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
     555        btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
     556        btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
    557557        isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
    558558        checkit(t0);checkit(t1);checkit(t2);checkit(t3);
    … …  
    560560        for(j=0;j<m_tris.size();j++)
    561561        {
    562                 Tri *t=m_tris[j];
     562                btHullTriangle *t=m_tris[j];
    563563                btAssert(t);
    564564                btAssert(t->vmax<0);
    … …  
    567567                t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
    568568        }
    569         Tri *te;
     569        btHullTriangle *te;
    570570        vlimit-=4;
    571571        while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
    … …  
    595595                        if(above(verts,nt,center,btScalar(0.01)*epsilon)  || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
    596596                        {
    597                                 Tri *nb = m_tris[m_tris[j]->n[0]];
     597                                btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
    598598                                btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
    599599                                extrude(nb,v);
    … …  
    604604                while(j--)
    605605                {
    606                         Tri *t=m_tris[j];
     606                        btHullTriangle *t=m_tris[j];
    607607                        if(!t) continue;
    608608                        if(t->vmax>=0) break;

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

    r2192 r2430  
    138138        ConvexH()
    139139        {
    140                 int i;
    141                 i=0;
    142140        }
    143141        ~ConvexH()
    144142        {
    145                 int i;
    146                 i=0;
    147143        }
    148144        btAlignedObjectArray<btVector3> vertices;
    … …  
    189185{
    190186
    191         btAlignedObjectArray<class Tri*> m_tris;
     187        btAlignedObjectArray<class btHullTriangle*> m_tris;
    192188
    193189public:
    … …  
    204200        bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
    205201
    206         class Tri*      allocateTriangle(int a,int b,int c);
    207         void    deAllocateTriangle(Tri*);
    208         void b2bfix(Tri* s,Tri*t);
    209 
    210         void removeb2b(Tri* s,Tri*t);
    211 
    212         void checkit(Tri *t);
    213 
    214         Tri* extrudable(btScalar epsilon);
     202        class btHullTriangle*   allocateTriangle(int a,int b,int c);
     203        void    deAllocateTriangle(btHullTriangle*);
     204        void b2bfix(btHullTriangle* s,btHullTriangle*t);
     205
     206        void removeb2b(btHullTriangle* s,btHullTriangle*t);
     207
     208        void checkit(btHullTriangle *t);
     209
     210        btHullTriangle* extrudable(btScalar epsilon);
    215211
    216212        int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
    … …  
    222218        class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
    223219
    224         void extrude(class Tri* t0,int v);
     220        void extrude(class btHullTriangle* t0,int v);
    225221
    226222        ConvexH* test_cube();

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

    r2192 r2430  
    2424
    2525
    26 ///The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
    27 ///Make sure to only include a pure orthogonal matrix without scaling.
     26/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
     27 * Make sure to only include a pure orthogonal matrix without scaling. */
    2828class btMatrix3x3 {
    2929        public:
     30  /** @brief No initializaion constructor */
    3031                btMatrix3x3 () {}
    3132               
    3233//              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
    3334               
     35  /**@brief Constructor from Quaternion */
    3436                explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
    3537                /*
    … …  
    4042                }
    4143                */
     44  /** @brief Constructor with row major formatting */
    4245                btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
    4346                                  const btScalar& yx, const btScalar& yy, const btScalar& yz,
    … …  
    4851                                         zx, zy, zz);
    4952                }
    50                
     53  /** @brief Copy constructor */
    5154                SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
    5255                {
    … …  
    5558                        m_el[2] = other.m_el[2];
    5659                }
    57 
     60  /** @brief Assignment Operator */
    5861                SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
    5962                {
    … …  
    6467                }
    6568
     69  /** @brief Get a column of the matrix as a vector
     70   *  @param i Column number 0 indexed */
    6671                SIMD_FORCE_INLINE btVector3 getColumn(int i) const
    6772                {
    … …  
    7075               
    7176
    72 
     77  /** @brief Get a row of the matrix as a vector
     78   *  @param i Row number 0 indexed */
    7379                SIMD_FORCE_INLINE const btVector3& getRow(int i) const
    7480                {
     81                        btFullAssert(0 <= i && i < 3);
    7582                        return m_el[i];
    7683                }
    7784
    78 
     85  /** @brief Get a mutable reference to a row of the matrix as a vector
     86   *  @param i Row number 0 indexed */
    7987                SIMD_FORCE_INLINE btVector3&  operator[](int i)
    8088                {
    … …  
    8391                }
    8492               
     93  /** @brief Get a const reference to a row of the matrix as a vector
     94   *  @param i Row number 0 indexed */
    8595                SIMD_FORCE_INLINE const btVector3& operator[](int i) const
    8696                {
    … …  
    8999                }
    90100               
     101  /** @brief Multiply by the target matrix on the right
     102   *  @param m Rotation matrix to be applied
     103   * Equivilant to this = this * m */
    91104                btMatrix3x3& operator*=(const btMatrix3x3& m);
    92105               
    93        
     106  /** @brief Set from a carray of btScalars
     107   *  @param m A pointer to the beginning of an array of 9 btScalars */
    94108        void setFromOpenGLSubMatrix(const btScalar *m)
    95109                {
    … …  
    99113
    100114                }
    101 
     115  /** @brief Set the values of the matrix explicitly (row major)
     116   *  @param xx Top left
     117   *  @param xy Top Middle
     118   *  @param xz Top Right
     119   *  @param yx Middle Left
     120   *  @param yy Middle Middle
     121   *  @param yz Middle Right
     122   *  @param zx Bottom Left
     123   *  @param zy Bottom Middle
     124   *  @param zz Bottom Right*/
    102125                void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
    103126                                          const btScalar& yx, const btScalar& yy, const btScalar& yz,
    … …  
    108131                        m_el[2].setValue(zx,zy,zz);
    109132                }
    110  
     133
     134  /** @brief Set the matrix from a quaternion
     135   *  @param q The Quaternion to match */ 
    111136                void setRotation(const btQuaternion& q)
    112137                {
    … …  
    124149               
    125150
    126 
     151  /** @brief Set the matrix from euler angles using YPR around YXZ respectively
     152   *  @param yaw Yaw about Y axis
     153   *  @param pitch Pitch about X axis
     154   *  @param roll Roll about Z axis
     155   */
    127156                void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
    128157                {
    129 
    130                         btScalar cy(btCos(yaw));
    131                         btScalar  sy(btSin(yaw));
    132                         btScalar  cp(btCos(pitch));
    133                         btScalar  sp(btSin(pitch));
    134                         btScalar  cr(btCos(roll));
    135                         btScalar  sr(btSin(roll));
    136                         btScalar  cc = cy * cr;
    137                         btScalar  cs = cy * sr;
    138                         btScalar  sc = sy * cr;
    139                         btScalar  ss = sy * sr;
    140                         setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
    141                      cp * sr,       cp * cr,      -sp,
    142                                          sc + sp * cs, -ss + sp * cc,  cy * cp);
    143                
    144                 }
    145 
    146         /**
    147          * setEulerZYX
    148          * @param euler a const reference to a btVector3 of euler angles
     158                        setEulerZYX(roll, pitch, yaw);
     159                }
     160
     161        /** @brief Set the matrix from euler angles YPR around ZYX axes
     162         * @param eulerX Roll about X axis
     163         * @param eulerY Pitch around Y axis
     164         * @param eulerZ Yaw aboud Z axis
     165         *
    149166         * These angles are used to produce a rotation matrix. The euler
    150167         * angles are applied in ZYX order. I.e a vector is first rotated
    151168         * about X then Y and then Z
    152169         **/
    153        
    154         void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
     170        void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
     171  ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
    155172                btScalar ci ( btCos(eulerX));
    156173                btScalar cj ( btCos(eulerY));
    … …  
    169186        }
    170187
     188  /**@brief Set the matrix to the identity */
    171189                void setIdentity()
    172190                {
    … …  
    175193                                         btScalar(0.0), btScalar(0.0), btScalar(1.0));
    176194                }
    177    
     195  /**@brief Fill the values of the matrix into a 9 element array
     196   * @param m The array to be filled */
    178197                void getOpenGLSubMatrix(btScalar *m) const
    179198                {
    … …  
    192211                }
    193212
     213  /**@brief Get the matrix represented as a quaternion
     214   * @param q The quaternion which will be set */
    194215                void getRotation(btQuaternion& q) const
    195216                {
    … …  
    225246                        q.setValue(temp[0],temp[1],temp[2],temp[3]);
    226247                }
    227        
    228                 void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
     248
     249  /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
     250   * @param yaw Yaw around Y axis
     251   * @param pitch Pitch around X axis
     252   * @param roll around Z axis */       
     253                void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
    229254                {
    230255                       
    231                         if (btScalar(m_el[1].z()) < btScalar(1))
    232                         {
    233                                 if (btScalar(m_el[1].z()) > -btScalar(1))
    234                                 {
    235                                         yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
    236                                         pitch = btScalar(btAsin(-m_el[1].y()));
    237                                         roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
    238                                 }
    239                                 else
    240                                 {
    241                                         yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
    242                                         pitch = SIMD_HALF_PI;
    243                                         roll = btScalar(0.0);
    244                                 }
    245                         }
    246                         else
    247                         {
    248                                 yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
    249                                 pitch = -SIMD_HALF_PI;
    250                                 roll = btScalar(0.0);
    251                         }
    252                 }
    253                
    254 
    255        
     256                        // first use the normal calculus
     257                        yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
     258                        pitch = btScalar(btAsin(-m_el[2].x()));
     259                        roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
     260
     261                        // on pitch = +/-HalfPI
     262                        if (btFabs(pitch)==SIMD_HALF_PI)
     263                        {
     264                                if (yaw>0)
     265                                        yaw-=SIMD_PI;
     266                                else
     267                                        yaw+=SIMD_PI;
     268
     269                                if (roll>0)
     270                                        roll-=SIMD_PI;
     271                                else
     272                                        roll+=SIMD_PI;
     273                        }
     274                };
     275
     276
     277  /**@brief Get the matrix represented as euler angles around ZYX
     278   * @param yaw Yaw around X axis
     279   * @param pitch Pitch around Y axis
     280   * @param roll around X axis
     281   * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/   
     282  void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
     283  {
     284    struct Euler{btScalar yaw, pitch, roll;};
     285    Euler euler_out;
     286    Euler euler_out2; //second solution
     287    //get the pointer to the raw data
     288   
     289    // Check that pitch is not at a singularity
     290    if (btFabs(m_el[2].x()) >= 1)
     291    {
     292      euler_out.yaw = 0;
     293      euler_out2.yaw = 0;
     294       
     295      // From difference of angles formula
     296      btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
     297      if (m_el[2].x() > 0)  //gimbal locked up
     298      {
     299        euler_out.pitch = SIMD_PI / btScalar(2.0);
     300        euler_out2.pitch = SIMD_PI / btScalar(2.0);
     301        euler_out.roll = euler_out.pitch + delta;
     302        euler_out2.roll = euler_out.pitch + delta;
     303      }
     304      else // gimbal locked down
     305      {
     306        euler_out.pitch = -SIMD_PI / btScalar(2.0);
     307        euler_out2.pitch = -SIMD_PI / btScalar(2.0);
     308        euler_out.roll = -euler_out.pitch + delta;
     309        euler_out2.roll = -euler_out.pitch + delta;
     310      }
     311    }
     312    else
     313    {
     314      euler_out.pitch = - btAsin(m_el[2].x());
     315      euler_out2.pitch = SIMD_PI - euler_out.pitch;
     316       
     317      euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
     318                               m_el[2].z()/btCos(euler_out.pitch));
     319      euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
     320                                m_el[2].z()/btCos(euler_out2.pitch));
     321       
     322      euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
     323                              m_el[0].x()/btCos(euler_out.pitch));
     324      euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
     325                               m_el[0].x()/btCos(euler_out2.pitch));
     326    }
     327   
     328    if (solution_number == 1)
     329    {
     330                yaw = euler_out.yaw;
     331                pitch = euler_out.pitch;
     332                roll = euler_out.roll;
     333    }
     334    else
     335    {
     336                yaw = euler_out2.yaw;
     337                pitch = euler_out2.pitch;
     338                roll = euler_out2.roll;
     339    }
     340  }
     341
     342  /**@brief Create a scaled copy of the matrix
     343   * @param s Scaling vector The elements of the vector will scale each column */
    256344               
    257345                btMatrix3x3 scaled(const btVector3& s) const
    … …  
    262350                }
    263351
     352  /**@brief Return the determinant of the matrix */
    264353                btScalar            determinant() const;
     354  /**@brief Return the adjoint of the matrix */
    265355                btMatrix3x3 adjoint() const;
     356  /**@brief Return the matrix with all values non negative */
    266357                btMatrix3x3 absolute() const;
     358  /**@brief Return the transpose of the matrix */
    267359                btMatrix3x3 transpose() const;
     360  /**@brief Return the inverse of the matrix */
    268361                btMatrix3x3 inverse() const;
    269362               
    … …  
    285378               
    286379
    287                 ///diagonalizes this matrix by the Jacobi method. rot stores the rotation
    288                 ///from the coordinate system in which the matrix is diagonal to the original
    289                 ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
    290                 ///stops when all off-diagonal elements are less than the threshold multiplied
    291                 ///by the sum of the absolute values of the diagonal, or when maxSteps have
    292                 ///been executed. Note that this matrix is assumed to be symmetric.
     380  /**@brief diagonalizes this matrix by the Jacobi method.
     381   * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
     382   * coordinate system, i.e., old_this = rot * new_this * rot^T.
     383   * @param threshold See iteration
     384   * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
     385   * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
     386   *
     387   * Note that this matrix is assumed to be symmetric.
     388   */
    293389                void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
    294390                {
    … …  
    372468               
    373469        protected:
     470  /**@brief Calculate the matrix cofactor
     471   * @param r1 The first row to use for calculating the cofactor
     472   * @param c1 The first column to use for calculating the cofactor
     473   * @param r1 The second row to use for calculating the cofactor
     474   * @param c1 The second column to use for calculating the cofactor
     475   * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
     476   */
    374477                btScalar cofac(int r1, int c1, int r2, int c2) const
    375478                {
    376479                        return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
    377480                }
    378 
     481  ///Data storage for the matrix, each vector is a row of the matrix
    379482                btVector3 m_el[3];
    380483        };
    … …  
    495598*/
    496599
     600/**@brief Equality operator between two matrices
     601 * It will test all elements are equal.  */
    497602SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
    498603{

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

    r2192 r2430  
    1919#include "btScalar.h"
    2020#include "btMinMax.h"
    21 #include <math.h>
    2221
    2322
     23#if defined (__CELLOS_LV2) && defined (__SPU__)
     24#include <altivec.h>
     25#endif
    2426
    25 ///The btQuadWordStorage class is base class for btVector3 and btQuaternion.
    26 ///Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. todo: look into this
    27 ///ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
     27/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion.
     28 * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
     29 */
     30#ifndef USE_LIBSPE2
     31ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
     32#else
    2833class btQuadWordStorage
     34#endif
    2935{
    3036protected:
    3137
    32         btScalar        m_x;
    33         btScalar        m_y;
    34         btScalar        m_z;
    35         btScalar        m_unusedW;
    36 
     38#if defined (__SPU__) && defined (__CELLOS_LV2__)
     39        union {
     40                vec_float4 mVec128;
     41                btScalar        m_floats[4];
     42        };
    3743public:
     44        vec_float4      get128() const
     45        {
     46                return mVec128;
     47        }
     48#else //__CELLOS_LV2__ __SPU__
     49        btScalar        m_floats[4];
     50#endif //__CELLOS_LV2__ __SPU__
    3851
    3952};
    4053
    41 
    42 ///btQuadWord is base-class for vectors, points
     54/** @brief The btQuadWord is base-class for vectors, points */
    4355class   btQuadWord : public btQuadWordStorage
    4456{
    4557        public:
    46        
    47 //              SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_x)[i];   }     
    48 //              SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; }
     58 
    4959
    50                 SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; }
     60  /**@brief Return the x value */
     61                SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
     62  /**@brief Return the y value */
     63                SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
     64  /**@brief Return the z value */
     65                SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
     66  /**@brief Set the x value */
     67                SIMD_FORCE_INLINE void  setX(btScalar x) { m_floats[0] = x;};
     68  /**@brief Set the y value */
     69                SIMD_FORCE_INLINE void  setY(btScalar y) { m_floats[1] = y;};
     70  /**@brief Set the z value */
     71                SIMD_FORCE_INLINE void  setZ(btScalar z) { m_floats[2] = z;};
     72  /**@brief Set the w value */
     73                SIMD_FORCE_INLINE void  setW(btScalar w) { m_floats[3] = w;};
     74  /**@brief Return the x value */
     75                SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
     76  /**@brief Return the y value */
     77                SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
     78  /**@brief Return the z value */
     79                SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
     80  /**@brief Return the w value */
     81                SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
    5182
    52                 SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; }
     83        //SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i]; }     
     84        //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
     85        ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
     86        SIMD_FORCE_INLINE       operator       btScalar *()       { return &m_floats[0]; }
     87        SIMD_FORCE_INLINE       operator const btScalar *() const { return &m_floats[0]; }
    5388
    54                 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; }
     89        SIMD_FORCE_INLINE       bool    operator==(const btQuadWord& other) const
     90        {
     91                return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
     92        }
    5593
    56                 SIMD_FORCE_INLINE void  setX(btScalar x) { m_x = x;};
     94        SIMD_FORCE_INLINE       bool    operator!=(const btQuadWord& other) const
     95        {
     96                return !(*this == other);
     97        }
    5798
    58                 SIMD_FORCE_INLINE void  setY(btScalar y) { m_y = y;};
    59 
    60                 SIMD_FORCE_INLINE void  setZ(btScalar z) { m_z = z;};
    61 
    62                 SIMD_FORCE_INLINE void  setW(btScalar w) { m_unusedW = w;};
    63 
    64                 SIMD_FORCE_INLINE const btScalar& x() const { return m_x; }
    65 
    66                 SIMD_FORCE_INLINE const btScalar& y() const { return m_y; }
    67 
    68                 SIMD_FORCE_INLINE const btScalar& z() const { return m_z; }
    69 
    70                 SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; }
    71 
    72 
    73                 SIMD_FORCE_INLINE       operator       btScalar *()       { return &m_x; }
    74                 SIMD_FORCE_INLINE       operator const btScalar *() const { return &m_x; }
    75 
    76 
    77 
     99  /**@brief Set x,y,z and zero w
     100   * @param x Value of x
     101   * @param y Value of y
     102   * @param z Value of z
     103   */
    78104                SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z)
    79105                {
    80                         m_x=x;
    81                         m_y=y;
    82                         m_z=z;
    83                         m_unusedW = 0.f;
     106                        m_floats[0]=x;
     107                        m_floats[1]=y;
     108                        m_floats[2]=z;
     109                        m_floats[3] = 0.f;
    84110                }
    85111
    86112/*              void getValue(btScalar *m) const
    87113                {
    88                         m[0] = m_x;
    89                         m[1] = m_y;
    90                         m[2] = m_z;
     114                        m[0] = m_floats[0];
     115                        m[1] = m_floats[1];
     116                        m[2] = m_floats[2];
    91117                }
    92118*/
     119/**@brief Set the values
     120   * @param x Value of x
     121   * @param y Value of y
     122   * @param z Value of z
     123   * @param w Value of w
     124   */
    93125                SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
    94126                {
    95                         m_x=x;
    96                         m_y=y;
    97                         m_z=z;
    98                         m_unusedW=w;
     127                        m_floats[0]=x;
     128                        m_floats[1]=y;
     129                        m_floats[2]=z;
     130                        m_floats[3]=w;
    99131                }
    100 
     132  /**@brief No initialization constructor */
    101133                SIMD_FORCE_INLINE btQuadWord()
    102                 //      :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))
     134                //      :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
    103135                {
    104136                }
    105 
     137  /**@brief Copy constructor */
    106138                SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
    107139                {
    108140                        *((btQuadWordStorage*)this) = q;
    109141                }
    110 
     142  /**@brief Three argument constructor (zeros w)
     143   * @param x Value of x
     144   * @param y Value of y
     145   * @param z Value of z
     146   */
    111147                SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)           
    112148                {
    113                         m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f;
     149                        m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f;
    114150                }
    115151
     152/**@brief Initializing constructor
     153   * @param x Value of x
     154   * @param y Value of y
     155   * @param z Value of z
     156   * @param w Value of w
     157   */
    116158                SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
    117159                {
    118                         m_x = x, m_y = y, m_z = z, m_unusedW = w;
     160                        m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w;
    119161                }
    120162
    121 
     163  /**@brief Set each element to the max of the current values and the values of another btQuadWord
     164   * @param other The other btQuadWord to compare with
     165   */
    122166                SIMD_FORCE_INLINE void  setMax(const btQuadWord& other)
    123167                {
    124                         btSetMax(m_x, other.m_x);
    125                         btSetMax(m_y, other.m_y);
    126                         btSetMax(m_z, other.m_z);
    127                         btSetMax(m_unusedW, other.m_unusedW);
     168                        btSetMax(m_floats[0], other.m_floats[0]);
     169                        btSetMax(m_floats[1], other.m_floats[1]);
     170                        btSetMax(m_floats[2], other.m_floats[2]);
     171                        btSetMax(m_floats[3], other.m_floats[3]);
    128172                }
    129 
     173  /**@brief Set each element to the min of the current values and the values of another btQuadWord
     174   * @param other The other btQuadWord to compare with
     175   */
    130176                SIMD_FORCE_INLINE void  setMin(const btQuadWord& other)
    131177                {
    132                         btSetMin(m_x, other.m_x);
    133                         btSetMin(m_y, other.m_y);
    134                         btSetMin(m_z, other.m_z);
    135                         btSetMin(m_unusedW, other.m_unusedW);
     178                        btSetMin(m_floats[0], other.m_floats[0]);
     179                        btSetMin(m_floats[1], other.m_floats[1]);
     180                        btSetMin(m_floats[2], other.m_floats[2]);
     181                        btSetMin(m_floats[3], other.m_floats[3]);
    136182                }
    137183

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

    r2192 r2430  
    1818#define SIMD__QUATERNION_H_
    1919
     20
    2021#include "btVector3.h"
    2122
    22 ///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform.
     23/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
    2324class btQuaternion : public btQuadWord {
    2425public:
     26  /**@brief No initialization constructor */
    2527        btQuaternion() {}
    2628
    2729        //              template <typename btScalar>
    2830        //              explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
    29 
     31  /**@brief Constructor from scalars */
    3032        btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
    3133                : btQuadWord(x, y, z, w)
    3234        {}
    33 
     35  /**@brief Axis angle Constructor
     36   * @param axis The axis which the rotation is around
     37   * @param angle The magnitude of the rotation around the angle (Radians) */
    3438        btQuaternion(const btVector3& axis, const btScalar& angle)
    3539        {
    3640                setRotation(axis, angle);
    3741        }
    38 
     42  /**@brief Constructor from Euler angles
     43   * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
     44   * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
     45   * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
    3946        btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
    4047        {
     48#ifndef BT_EULER_DEFAULT_ZYX
    4149                setEuler(yaw, pitch, roll);
    42         }
    43 
     50#else
     51                setEulerZYX(yaw, pitch, roll);
     52#endif
     53        }
     54  /**@brief Set the rotation using axis angle notation
     55   * @param axis The axis around which to rotate
     56   * @param angle The magnitude of the rotation in Radians */
    4457        void setRotation(const btVector3& axis, const btScalar& angle)
    4558        {
    … …  
    5063                        btCos(angle * btScalar(0.5)));
    5164        }
    52 
     65  /**@brief Set the quaternion using Euler angles
     66   * @param yaw Angle around Y
     67   * @param pitch Angle around X
     68   * @param roll Angle around Z */
    5369        void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
    5470        {
    … …  
    6783                        cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
    6884        }
    69 
    70         btQuaternion& operator+=(const btQuaternion& q)
    71         {
    72                 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW;
     85  /**@brief Set the quaternion using euler angles
     86   * @param yaw Angle around Z
     87   * @param pitch Angle around Y
     88   * @param roll Angle around X */
     89        void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
     90        {
     91                btScalar halfYaw = btScalar(yaw) * btScalar(0.5); 
     92                btScalar halfPitch = btScalar(pitch) * btScalar(0.5); 
     93                btScalar halfRoll = btScalar(roll) * btScalar(0.5); 
     94                btScalar cosYaw = btCos(halfYaw);
     95                btScalar sinYaw = btSin(halfYaw);
     96                btScalar cosPitch = btCos(halfPitch);
     97                btScalar sinPitch = btSin(halfPitch);
     98                btScalar cosRoll = btCos(halfRoll);
     99                btScalar sinRoll = btSin(halfRoll);
     100                setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
     101                         cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
     102                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
     103                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
     104        }
     105  /**@brief Add two quaternions
     106   * @param q The quaternion to add to this one */
     107        SIMD_FORCE_INLINE       btQuaternion& operator+=(const btQuaternion& q)
     108        {
     109                m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
    73110                return *this;
    74111        }
    75112
     113  /**@brief Subtract out a quaternion
     114   * @param q The quaternion to subtract from this one */
    76115        btQuaternion& operator-=(const btQuaternion& q)
    77116        {
    78                 m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;
     117                m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
    79118                return *this;
    80119        }
    81120
     121  /**@brief Scale this quaternion
     122   * @param s The scalar to scale by */
    82123        btQuaternion& operator*=(const btScalar& s)
    83124        {
    84                 m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
     125                m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
    85126                return *this;
    86127        }
    87128
    88 
     129  /**@brief Multiply this quaternion by q on the right
     130   * @param q The other quaternion
     131   * Equivilant to this = this * q */
    89132        btQuaternion& operator*=(const btQuaternion& q)
    90133        {
    91                 setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(),
    92                         m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(),
    93                         m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
    94                         m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
     134                setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
     135                        m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
     136                        m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
     137                        m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
    95138                return *this;
    96139        }
    97 
     140  /**@brief Return the dot product between this quaternion and another
     141   * @param q The other quaternion */
    98142        btScalar dot(const btQuaternion& q) const
    99143        {
    100                 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW;
    101         }
    102 
     144                return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
     145        }
     146
     147  /**@brief Return the length squared of the quaternion */
    103148        btScalar length2() const
    104149        {
    … …  
    106151        }
    107152
     153  /**@brief Return the length of the quaternion */
    108154        btScalar length() const
    109155        {
    … …  
    111157        }
    112158
     159  /**@brief Normalize the quaternion
     160   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
    113161        btQuaternion& normalize()
    114162        {
    … …  
    116164        }
    117165
     166  /**@brief Return a scaled version of this quaternion
     167   * @param s The scale factor */
    118168        SIMD_FORCE_INLINE btQuaternion
    119169        operator*(const btScalar& s) const
    120170        {
    121                 return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
    122         }
    123 
    124 
    125 
     171                return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
     172        }
     173
     174
     175  /**@brief Return an inversely scaled versionof this quaternion
     176   * @param s The inverse scale factor */
    126177        btQuaternion operator/(const btScalar& s) const
    127178        {
    … …  
    130181        }
    131182
    132 
     183  /**@brief Inversely scale this quaternion
     184   * @param s The scale factor */
    133185        btQuaternion& operator/=(const btScalar& s)
    134186        {
    … …  
    137189        }
    138190
    139 
     191  /**@brief Return a normalized version of this quaternion */
    140192        btQuaternion normalized() const
    141193        {
    142194                return *this / length();
    143195        }
    144 
     196  /**@brief Return the angle between this quaternion and the other
     197   * @param q The other quaternion */
    145198        btScalar angle(const btQuaternion& q) const
    146199        {
    … …  
    149202                return btAcos(dot(q) / s);
    150203        }
    151 
     204  /**@brief Return the angle of rotation represented by this quaternion */
    152205        btScalar getAngle() const
    153206        {
    154                 btScalar s = btScalar(2.) * btAcos(m_unusedW);
     207                btScalar s = btScalar(2.) * btAcos(m_floats[3]);
    155208                return s;
    156209        }
    157210
    158211
    159 
     212  /**@brief Return the inverse of this quaternion */
    160213        btQuaternion inverse() const
    161214        {
    162                 return btQuaternion(-m_x, -m_y, -m_z, m_unusedW);
    163         }
    164 
     215                return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
     216        }
     217
     218  /**@brief Return the sum of this quaternion and the other
     219   * @param q2 The other quaternion */
    165220        SIMD_FORCE_INLINE btQuaternion
    166221        operator+(const btQuaternion& q2) const
    167222        {
    168223                const btQuaternion& q1 = *this;
    169                 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW);
    170         }
    171 
     224                return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
     225        }
     226
     227  /**@brief Return the difference between this quaternion and the other
     228   * @param q2 The other quaternion */
    172229        SIMD_FORCE_INLINE btQuaternion
    173230        operator-(const btQuaternion& q2) const
    174231        {
    175232                const btQuaternion& q1 = *this;
    176                 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW);
    177         }
    178 
     233                return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
     234        }
     235
     236  /**@brief Return the negative of this quaternion
     237   * This simply negates each element */
    179238        SIMD_FORCE_INLINE btQuaternion operator-() const
    180239        {
    181240                const btQuaternion& q2 = *this;
    182                 return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_unusedW);
    183         }
    184 
     241                return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
     242        }
     243  /**@todo document this and it's use */
    185244        SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
    186245        {
    … …  
    193252        }
    194253
     254  /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
     255   * @param q The other quaternion to interpolate with
     256   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
     257   * Slerp interpolates assuming constant velocity.  */
    195258        btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
    196259        {
    … …  
    201264                        btScalar s0 = btSin((btScalar(1.0) - t) * theta);
    202265                        btScalar s1 = btSin(t * theta);   
    203                         return btQuaternion((m_x * s0 + q.x() * s1) * d,
    204                                 (m_y * s0 + q.y() * s1) * d,
    205                                 (m_z * s0 + q.z() * s1) * d,
    206                                 (m_unusedW * s0 + q.m_unusedW * s1) * d);
     266                        return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
     267                                (m_floats[1] * s0 + q.y() * s1) * d,
     268                                (m_floats[2] * s0 + q.z() * s1) * d,
     269                                (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
    207270                }
    208271                else
    … …  
    212275        }
    213276
    214         SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; }
     277        SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
    215278
    216279       
    … …  
    218281
    219282
    220 
     283/**@brief Return the negative of a quaternion */
    221284SIMD_FORCE_INLINE btQuaternion
    222285operator-(const btQuaternion& q)
    … …  
    227290
    228291
    229 
     292/**@brief Return the product of two quaternions */
    230293SIMD_FORCE_INLINE btQuaternion
    231294operator*(const btQuaternion& q1, const btQuaternion& q2) {
    … …  
    254317}
    255318
     319/**@brief Calculate the dot product between two quaternions */
    256320SIMD_FORCE_INLINE btScalar
    257321dot(const btQuaternion& q1, const btQuaternion& q2)
    … …  
    261325
    262326
     327/**@brief Return the length of a quaternion */
    263328SIMD_FORCE_INLINE btScalar
    264329length(const btQuaternion& q)
    … …  
    267332}
    268333
     334/**@brief Return the angle between two quaternions*/
    269335SIMD_FORCE_INLINE btScalar
    270336angle(const btQuaternion& q1, const btQuaternion& q2)
    … …  
    273339}
    274340
    275 
     341/**@brief Return the inverse of a quaternion*/
    276342SIMD_FORCE_INLINE btQuaternion
    277343inverse(const btQuaternion& q)
    … …  
    280346}
    281347
     348/**@brief Return the result of spherical linear interpolation betwen two quaternions
     349 * @param q1 The first quaternion
     350 * @param q2 The second quaternion
     351 * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2
     352 * Slerp assumes constant velocity between positions. */
    282353SIMD_FORCE_INLINE btQuaternion
    283354slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)

  • code/branches/physics/src/bullet/LinearMath/btQuickprof.cpp

    r2192 r2430  
    111111        TotalCalls = 0;
    112112        TotalTime = 0.0f;
    113         gProfileClock.reset();
     113       
    114114
    115115        if ( Child ) {
    … …  
    252252void    CProfileManager::Reset( void )
    253253{
     254        gProfileClock.reset();
    254255        Root.Reset();
    255256    Root.Call();
    … …  
    279280}
    280281
     282#include <stdio.h>
     283
     284void    CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing)
     285{
     286        profileIterator->First();
     287        if (profileIterator->Is_Done())
     288                return;
     289
     290        float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
     291        int i;
     292        int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
     293        for (i=0;i<spacing;i++) printf(".");
     294        printf("----------------------------------\n");
     295        for (i=0;i<spacing;i++) printf(".");
     296        printf("Profiling: %s (total running time: %.3f ms) ---\n",     profileIterator->Get_Current_Parent_Name(), parent_time );
     297        float totalTime = 0.f;
     298
     299       
     300        int numChildren = 0;
     301       
     302        for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next())
     303        {
     304                numChildren++;
     305                float current_total_time = profileIterator->Get_Current_Total_Time();
     306                accumulated_time += current_total_time;
     307                float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f;
     308                {
     309                        int i;  for (i=0;i<spacing;i++) printf(".");
     310                }
     311                printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n",i, profileIterator->Get_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls());
     312                totalTime += current_total_time;
     313                //recurse into children
     314        }
     315
     316        if (parent_time < accumulated_time)
     317        {
     318                printf("what's wrong\n");
     319        }
     320        for (i=0;i<spacing;i++) printf(".");
     321        printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
     322       
     323        for (i=0;i<numChildren;i++)
     324        {
     325                profileIterator->Enter_Child(i);
     326                dumpRecursive(profileIterator,spacing+3);
     327                profileIterator->Enter_Parent();
     328        }
     329}
     330
     331
     332
     333void    CProfileManager::dumpAll()
     334{
     335        CProfileIterator* profileIterator = 0;
     336        profileIterator = CProfileManager::Get_Iterator();
     337
     338        dumpRecursive(profileIterator,0);
     339
     340        CProfileManager::Release_Iterator(profileIterator);
     341}
     342
     343
     344
    281345#endif //USE_BT_CLOCK
    282346

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

    r2192 r2430  
    1111// Ogre (www.ogre3d.org).
    1212
     13
     14
    1315#ifndef QUICK_PROF_H
    1416#define QUICK_PROF_H
     17
     18//To disable built-in profiling, please comment out next line
     19//#define BT_NO_PROFILE 1
     20#ifndef BT_NO_PROFILE
    1521
    1622#include "btScalar.h"
    1723#include "LinearMath/btAlignedAllocator.h"
    1824#include <new>
    19 //To disable built-in profiling, please comment out next line
    20 //#define BT_NO_PROFILE 1
     25
     26
    2127
    2228
    … …  
    322328        static  void                                            Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
    323329
     330        static void     dumpRecursive(CProfileIterator* profileIterator, int spacing);
     331
     332        static void     dumpAll();
     333
    324334private:
    325335        static  CProfileNode                    Root;
    … …  
    345355};
    346356
    347 #if !defined(BT_NO_PROFILE)
     357
    348358#define BT_PROFILE( name )                      CProfileSample __profile( name )
    349 #else
     359
     360#else
     361
    350362#define BT_PROFILE( name )
    351 #endif
    352 
     363
     364#endif //#ifndef BT_NO_PROFILE
    353365
    354366

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

    r2192 r2430  
    1919
    2020#include <math.h>
     21
    2122#include <stdlib.h>//size_t for MSVC 6.0
    2223
    … …  
    2526#include <float.h>
    2627
    27 #define BT_BULLET_VERSION 272
     28#define BT_BULLET_VERSION 273
    2829
    2930inline int      btGetVersion()
    … …  
    6667
    6768                #include <assert.h>
    68 #if defined(DEBUG) || defined (_DEBUG)
     69#ifdef BT_DEBUG
    6970                #define btAssert assert
    7071#else
    … …  
    8687                #include <assert.h>
    8788                #endif
     89#ifdef BT_DEBUG
    8890                #define btAssert assert
     91#else
     92                #define btAssert(x)
     93#endif
    8994                //btFullAssert is optional, slows down a lot
    9095                #define btFullAssert(x)
    … …  
    103108                #include <assert.h>
    104109                #endif
     110#ifdef BT_DEBUG
    105111                #define btAssert assert
     112#else
     113                #define btAssert(x)
     114#endif
    106115                //btFullAssert is optional, slows down a lot
    107116                #define btFullAssert(x)

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

    r2192 r2430  
    2424#include "btAlignedAllocator.h"
    2525
     26///The btBlock class is an internal structure for the btStackAlloc memory allocator.
    2627struct btBlock
    2728{

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

    r2192 r2430  
    2222
    2323
    24 ///The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
    25 ///It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes.
     24/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
     25 *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */
    2626class btTransform {
    2727       
    … …  
    2929public:
    3030       
    31        
     31  /**@brief No initialization constructor */
    3232        btTransform() {}
    33 
     33  /**@brief Constructor from btQuaternion (optional btVector3 )
     34   * @param q Rotation from quaternion
     35   * @param c Translation from Vector (default 0,0,0) */
    3436        explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
    3537                const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
    … …  
    3840        {}
    3941
     42  /**@brief Constructor from btMatrix3x3 (optional btVector3)
     43   * @param b Rotation from Matrix
     44   * @param c Translation from Vector default (0,0,0)*/
    4045        explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
    4146                const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
    … …  
    4348                m_origin(c)
    4449        {}
    45 
     50  /**@brief Copy constructor */
    4651        SIMD_FORCE_INLINE btTransform (const btTransform& other)
    4752                : m_basis(other.m_basis),
    … …  
    4954        {
    5055        }
    51 
     56  /**@brief Assignment Operator */
    5257        SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
    5358        {
    … …  
    5863
    5964
     65  /**@brief Set the current transform as the value of the product of two transforms
     66   * @param t1 Transform 1
     67   * @param t2 Transform 2
     68   * This = Transform1 * Transform2 */
    6069                SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
    6170                        m_basis = t1.m_basis * t2.m_basis;
    … …  
    7079                */
    7180
    72 
     81/**@brief Return the transform of the vector */
    7382        SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
    7483        {
    … …  
    7887        }
    7988
     89  /**@brief Return the transform of the vector */
    8090        SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
    8191        {
    … …  
    8393        }
    8494
     95  /**@brief Return the transform of the btQuaternion */
     96        SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const
     97        {
     98                return getRotation() * q;
     99        }
     100
     101  /**@brief Return the basis matrix for the rotation */
    85102        SIMD_FORCE_INLINE btMatrix3x3&       getBasis()          { return m_basis; }
     103  /**@brief Return the basis matrix for the rotation */
    86104        SIMD_FORCE_INLINE const btMatrix3x3& getBasis()    const { return m_basis; }
    87105
     106  /**@brief Return the origin vector translation */
    88107        SIMD_FORCE_INLINE btVector3&         getOrigin()         { return m_origin; }
     108  /**@brief Return the origin vector translation */
    89109        SIMD_FORCE_INLINE const btVector3&   getOrigin()   const { return m_origin; }
    90110
     111  /**@brief Return a quaternion representing the rotation */
    91112        btQuaternion getRotation() const {
    92113                btQuaternion q;
    … …  
    96117       
    97118       
     119  /**@brief Set from an array
     120   * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
    98121        void setFromOpenGLMatrix(const btScalar *m)
    99122        {
    … …  
    102125        }
    103126
     127  /**@brief Fill an array representation
     128   * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
    104129        void getOpenGLMatrix(btScalar *m) const
    105130        {
    … …  
    111136        }
    112137
     138  /**@brief Set the translational element
     139   * @param origin The vector to set the translation to */
    113140        SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
    114141        {
    … …  
    119146
    120147
    121 
     148  /**@brief Set the rotational element by btMatrix3x3 */
    122149        SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
    123150        {
    … …  
    125152        }
    126153
     154  /**@brief Set the rotational element by btQuaternion */
    127155        SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
    128156        {
    … …  
    131159
    132160
    133 
     161  /**@brief Set this transformation to the identity */
    134162        void setIdentity()
    135163        {
    … …  
    138166        }
    139167
    140        
     168  /**@brief Multiply this Transform by another(this = this * another)
     169   * @param t The other transform */
    141170        btTransform& operator*=(const btTransform& t)
    142171        {
    … …  
    146175        }
    147176
     177  /**@brief Return the inverse of this transform */
    148178        btTransform inverse() const
    149179        {
    … …  
    152182        }
    153183
     184  /**@brief Return the inverse of this transform times the other transform
     185   * @param t The other transform
     186   * return this.inverse() * the other */
    154187        btTransform inverseTimes(const btTransform& t) const; 
    155188
     189  /**@brief Return the product of this transform and the other */
    156190        btTransform operator*(const btTransform& t) const;
    157191
     192  /**@brief Return an identity transform */
    158193        static btTransform      getIdentity()
    159194        {
    … …  
    164199       
    165200private:
    166 
     201  ///Storage for the rotation
    167202        btMatrix3x3 m_basis;
     203  ///Storage for the translation
    168204        btVector3   m_origin;
    169205};
    … …  
    192228}
    193229
     230/**@brief Test if two transforms have all elements equal */
    194231SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2)
    195232{

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

    r2192 r2430  
    101101        }
    102102
     103        static void     calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
     104        {
     105                linVel = (pos1 - pos0) / timeStep;
     106                btVector3 axis;
     107                btScalar  angle;
     108                if (orn0 != orn1)
     109                {
     110                        calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
     111                        angVel = axis * angle / timeStep;
     112                } else
     113                {
     114                        angVel.setValue(0,0,0);
     115                }
     116        }
     117
     118        static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
     119        {
     120                btQuaternion orn1 = orn0.farthest(orn1a);
     121                btQuaternion dorn = orn1 * orn0.inverse();
     122                ///floating point inaccuracy can lead to w component > 1..., which breaks
     123                dorn.normalize();
     124                angle = dorn.getAngle();
     125                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
     126                axis[3] = btScalar(0.);
     127                //check for axis length
     128                btScalar len = axis.length2();
     129                if (len < SIMD_EPSILON*SIMD_EPSILON)
     130                        axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
     131                else
     132                        axis /= btSqrt(len);
     133        }
     134
    103135        static void     calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
    104136        {
    … …  
    112144        static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
    113145        {
    114        
    115         #ifdef USE_QUATERNION_DIFF
    116                 btQuaternion orn0 = transform0.getRotation();
    117                 btQuaternion orn1a = transform1.getRotation();
    118                 btQuaternion orn1 = orn0.farthest(orn1a);
    119                 btQuaternion dorn = orn1 * orn0.inverse();
    120 #else
    121146                btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
    122147                btQuaternion dorn;
    123148                dmat.getRotation(dorn);
    124 #endif//USE_QUATERNION_DIFF
    125        
     149
    126150                ///floating point inaccuracy can lead to w component > 1..., which breaks
    127 
    128151                dorn.normalize();
    129152               
    … …  
    141164};
    142165
     166
     167///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
     168///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
     169class   btConvexSeparatingDistanceUtil
     170{
     171        btQuaternion    m_ornA;
     172        btQuaternion    m_ornB;
     173        btVector3       m_posA;
     174        btVector3       m_posB;
     175       
     176        btVector3       m_separatingNormal;
     177
     178        btScalar        m_boundingRadiusA;
     179        btScalar        m_boundingRadiusB;
     180        btScalar        m_separatingDistance;
     181
     182public:
     183
     184        btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar        boundingRadiusB)
     185                :m_boundingRadiusA(boundingRadiusA),
     186                m_boundingRadiusB(boundingRadiusB),
     187                m_separatingDistance(0.f)
     188        {
     189        }
     190
     191        btScalar        getConservativeSeparatingDistance()
     192        {
     193                return m_separatingDistance;
     194        }
     195
     196        void    updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
     197        {
     198                const btVector3& toPosA = transA.getOrigin();
     199                const btVector3& toPosB = transB.getOrigin();
     200                btQuaternion toOrnA = transA.getRotation();
     201                btQuaternion toOrnB = transB.getRotation();
     202
     203                if (m_separatingDistance>0.f)
     204                {
     205                       
     206
     207                        btVector3 linVelA,angVelA,linVelB,angVelB;
     208                        btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
     209                        btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
     210                        btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
     211                        btVector3 relLinVel = (linVelB-linVelA);
     212                        btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
     213                        if (relLinVelocLength<0.f)
     214                        {
     215                                relLinVelocLength = 0.f;
     216                        }
     217       
     218                        btScalar        projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
     219                        m_separatingDistance -= projectedMotion;
     220                }
     221       
     222                m_posA = toPosA;
     223                m_posB = toPosB;
     224                m_ornA = toOrnA;
     225                m_ornB = toOrnB;
     226        }
     227
     228        void    initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
     229        {
     230                m_separatingNormal = separatingVector;
     231                m_separatingDistance = separatingDistance;
     232               
     233                const btVector3& toPosA = transA.getOrigin();
     234                const btVector3& toPosB = transB.getOrigin();
     235                btQuaternion toOrnA = transA.getRotation();
     236                btQuaternion toOrnB = transB.getRotation();
     237                m_posA = toPosA;
     238                m_posB = toPosB;
     239                m_ornA = toOrnA;
     240                m_ornB = toOrnB;
     241        }
     242
     243};
     244
     245
    143246#endif //SIMD_TRANSFORM_UTIL_H
    144247

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

    r2192 r2430  
    2020#include "btQuadWord.h"
    2121
    22 ///btVector3 can be used to represent 3D points and vectors.
    23 ///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
    24 ///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
     22/**@brief btVector3 can be used to represent 3D points and vectors.
     23 * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
     24 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
     25 */
    2526class   btVector3 : public btQuadWord {
    2627
    2728public:
     29  /**@brief No initialization constructor */
    2830        SIMD_FORCE_INLINE btVector3() {}
    2931
     32  /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
     33   * Note: Vector3 derives from btQuadWordStorage*/
    3034        SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q)
    3135                : btQuadWord(q)
    … …  
    3337        }
    3438       
    35 
     39  /**@brief Constructor from scalars
     40   * @param x X value
     41   * @param y Y value
     42   * @param z Z value
     43   */
    3644        SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
    3745                :btQuadWord(x,y,z,btScalar(0.))
    … …  
    4553
    4654       
    47 
     55/**@brief Add a vector to this one
     56 * @param The vector to add to this one */
    4857        SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
    4958        {
    5059
    51                 m_x += v.x(); m_y += v.y(); m_z += v.z();
     60                m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
    5261                return *this;
    5362        }
    5463
    5564
    56 
     65  /**@brief Subtract a vector from this one
     66   * @param The vector to subtract */
    5767        SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
    5868        {
    59                 m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
     69                m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
    6070                return *this;
    6171        }
    62 
     72  /**@brief Scale the vector
     73   * @param s Scale factor */
    6374        SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
    6475        {
    65                 m_x *= s; m_y *= s; m_z *= s;
     76                m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
    6677                return *this;
    6778        }
    6879
     80  /**@brief Inversely scale the vector
     81   * @param s Scale factor to divide by */
    6982        SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
    7083        {
    … …  
    7386        }
    7487
     88  /**@brief Return the dot product
     89   * @param v The other vector in the dot product */
    7590        SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
    7691        {
    77                 return m_x * v.x() + m_y * v.y() + m_z * v.z();
    78         }
    79 
     92                return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
     93        }
     94
     95  /**@brief Return the length of the vector squared */
    8096        SIMD_FORCE_INLINE btScalar length2() const
    8197        {
    … …  
    8399        }
    84100
     101  /**@brief Return the length of the vector */
    85102        SIMD_FORCE_INLINE btScalar length() const
    86103        {
    … …  
    88105        }
    89106
     107  /**@brief Return the distance squared between the ends of this and another vector
     108   * This is symantically treating the vector like a point */
    90109        SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
    91110
     111  /**@brief Return the distance between the ends of this and another vector
     112   * This is symantically treating the vector like a point */
    92113        SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
    93114
     115  /**@brief Normalize this vector
     116   * x^2 + y^2 + z^2 = 1 */
    94117        SIMD_FORCE_INLINE btVector3& normalize()
    95118        {
    … …  
    97120        }
    98121
     122  /**@brief Return a normalized version of this vector */
    99123        SIMD_FORCE_INLINE btVector3 normalized() const;
    100124
     125  /**@brief Rotate this vector
     126   * @param wAxis The axis to rotate about
     127   * @param angle The angle to rotate by */
    101128        SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
    102129
     130  /**@brief Return the angle between this and another vector
     131   * @param v The other vector */
    103132        SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
    104133        {
    … …  
    107136                return btAcos(dot(v) / s);
    108137        }
    109 
     138  /**@brief Return a vector will the absolute values of each element */
    110139        SIMD_FORCE_INLINE btVector3 absolute() const
    111140        {
    112141                return btVector3(
    113                         btFabs(m_x),
    114                         btFabs(m_y),
    115                         btFabs(m_z));
    116         }
    117 
     142                        btFabs(m_floats[0]),
     143                        btFabs(m_floats[1]),
     144                        btFabs(m_floats[2]));
     145        }
     146  /**@brief Return the cross product between this and another vector
     147   * @param v The other vector */
    118148        SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
    119149        {
    120150                return btVector3(
    121                         m_y * v.z() - m_z * v.y(),
    122                         m_z * v.x() - m_x * v.z(),
    123                         m_x * v.y() - m_y * v.x());
     151                        m_floats[1] * v.z() - m_floats[2] * v.y(),
     152                        m_floats[2] * v.x() - m_floats[0] * v.z(),
     153                        m_floats[0] * v.y() - m_floats[1] * v.x());
    124154        }
    125155
    126156        SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
    127157        {
    128                 return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) +
    129                         m_y * (v1.z() * v2.x() - v1.x() * v2.z()) +
    130                         m_z * (v1.x() * v2.y() - v1.y() * v2.x());
    131         }
    132 
     158                return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) +
     159                        m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) +
     160                        m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
     161        }
     162
     163  /**@brief Return the axis with the smallest value
     164   * Note return values are 0,1,2 for x, y, or z */
    133165        SIMD_FORCE_INLINE int minAxis() const
    134166        {
    135                 return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
    136         }
    137 
     167                return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
     168        }
     169
     170  /**@brief Return the axis with the largest value
     171   * Note return values are 0,1,2 for x, y, or z */
    138172        SIMD_FORCE_INLINE int maxAxis() const
    139173        {
    140                 return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
     174                return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
    141175        }
    142176
    … …  
    154188        {
    155189                btScalar s = btScalar(1.0) - rt;
    156                 m_x = s * v0.x() + rt * v1.x();
    157                 m_y = s * v0.y() + rt * v1.y();
    158                 m_z = s * v0.z() + rt * v1.z();
     190                m_floats[0] = s * v0.x() + rt * v1.x();
     191                m_floats[1] = s * v0.y() + rt * v1.y();
     192                m_floats[2] = s * v0.z() + rt * v1.z();
    159193                //don't do the unused w component
    160194                //              m_co[3] = s * v0[3] + rt * v1[3];
    161195        }
    162196
     197  /**@brief Return the linear interpolation between this and another vector
     198   * @param v The other vector
     199   * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
    163200        SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
    164201        {
    165                 return btVector3(m_x + (v.x() - m_x) * t,
    166                         m_y + (v.y() - m_y) * t,
    167                         m_z + (v.z() - m_z) * t);
    168         }
    169 
    170 
     202                return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
     203                        m_floats[1] + (v.y() - m_floats[1]) * t,
     204                        m_floats[2] + (v.z() - m_floats[2]) * t);
     205        }
     206
     207  /**@brief Elementwise multiply this vector by the other
     208   * @param v The other vector */
    171209        SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
    172210        {
    173                 m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
     211                m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
    174212                return *this;
    175213        }
    … …  
    179217};
    180218
     219/**@brief Return the sum of two vectors (Point symantics)*/
    181220SIMD_FORCE_INLINE btVector3
    182221operator+(const btVector3& v1, const btVector3& v2)
    … …  
    185224}
    186225
     226/**@brief Return the elementwise product of two vectors */
    187227SIMD_FORCE_INLINE btVector3
    188228operator*(const btVector3& v1, const btVector3& v2)
    … …  
    191231}
    192232
     233/**@brief Return the difference between two vectors */
    193234SIMD_FORCE_INLINE btVector3
    194235operator-(const btVector3& v1, const btVector3& v2)
    … …  
    196237        return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
    197238}
    198 
     239/**@brief Return the negative of the vector */
    199240SIMD_FORCE_INLINE btVector3
    200241operator-(const btVector3& v)
    … …  
    203244}
    204245
     246/**@brief Return the vector scaled by s */
    205247SIMD_FORCE_INLINE btVector3
    206248operator*(const btVector3& v, const btScalar& s)
    … …  
    209251}
    210252
     253/**@brief Return the vector scaled by s */
    211254SIMD_FORCE_INLINE btVector3
    212255operator*(const btScalar& s, const btVector3& v)
    … …  
    215258}
    216259
     260/**@brief Return the vector inversely scaled by s */
    217261SIMD_FORCE_INLINE btVector3
    218262operator/(const btVector3& v, const btScalar& s)
    … …  
    222266}
    223267
     268/**@brief Return the vector inversely scaled by s */
    224269SIMD_FORCE_INLINE btVector3
    225270operator/(const btVector3& v1, const btVector3& v2)
    … …  
    228273}
    229274
     275/**@brief Return the dot product between two vectors */
    230276SIMD_FORCE_INLINE btScalar
    231277dot(const btVector3& v1, const btVector3& v2)
    … …  
    235281
    236282
    237 
     283/**@brief Return the distance squared between two vectors */
    238284SIMD_FORCE_INLINE btScalar
    239285distance2(const btVector3& v1, const btVector3& v2)
    … …  
    243289
    244290
     291/**@brief Return the distance between two vectors */
    245292SIMD_FORCE_INLINE btScalar
    246293distance(const btVector3& v1, const btVector3& v2)
    … …  
    249296}
    250297
     298/**@brief Return the angle between two vectors */
    251299SIMD_FORCE_INLINE btScalar
    252300angle(const btVector3& v1, const btVector3& v2)
    … …  
    255303}
    256304
     305/**@brief Return the cross product of two vectors */
    257306SIMD_FORCE_INLINE btVector3
    258307cross(const btVector3& v1, const btVector3& v2)
    … …  
    267316}
    268317
     318/**@brief Return the linear interpolation between two vectors
     319 * @param v1 One vector
     320 * @param v2 The other vector
     321 * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
    269322SIMD_FORCE_INLINE btVector3
    270323lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
    … …  
    273326}
    274327
    275 
     328/**@brief Test if each element of the vector is equivalent */
    276329SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2)
    277330{
    … …  
    317370                : btVector3(x,y,z)
    318371        {
    319                 m_unusedW = w;
     372                m_floats[3] = w;
    320373        }
    321374
    … …  
    324377        {
    325378                return btVector4(
    326                         btFabs(m_x),
    327                         btFabs(m_y),
    328                         btFabs(m_z),
    329                         btFabs(m_unusedW));
    330         }
    331 
    332 
    333 
    334         btScalar        getW() const { return m_unusedW;}
     379                        btFabs(m_floats[0]),
     380                        btFabs(m_floats[1]),
     381                        btFabs(m_floats[2]),
     382                        btFabs(m_floats[3]));
     383        }
     384
     385
     386
     387        btScalar        getW() const { return m_floats[3];}
    335388
    336389
    … …  
    339392                int maxIndex = -1;
    340393                btScalar maxVal = btScalar(-1e30);
    341                 if (m_x > maxVal)
     394                if (m_floats[0] > maxVal)
    342395                {
    343396                        maxIndex = 0;
    344                         maxVal = m_x;
    345                 }
    346                 if (m_y > maxVal)
     397                        maxVal = m_floats[0];
     398                }
     399                if (m_floats[1] > maxVal)
    347400                {
    348401                        maxIndex = 1;
    349                         maxVal = m_y;
    350                 }
    351                 if (m_z > maxVal)
     402                        maxVal = m_floats[1];
     403                }
     404                if (m_floats[2] > maxVal)
    352405                {
    353406                        maxIndex = 2;
    354                         maxVal = m_z;
    355                 }
    356                 if (m_unusedW > maxVal)
     407                        maxVal = m_floats[2];
     408                }
     409                if (m_floats[3] > maxVal)
    357410                {
    358411                        maxIndex = 3;
    359                         maxVal = m_unusedW;
     412                        maxVal = m_floats[3];
    360413                }
    361414               
    … …  
    372425                int minIndex = -1;
    373426                btScalar minVal = btScalar(1e30);
    374                 if (m_x < minVal)
     427                if (m_floats[0] < minVal)
    375428                {
    376429                        minIndex = 0;
    377                         minVal = m_x;
    378                 }
    379                 if (m_y < minVal)
     430                        minVal = m_floats[0];
     431                }
     432                if (m_floats[1] < minVal)
    380433                {
    381434                        minIndex = 1;
    382                         minVal = m_y;
    383                 }
    384                 if (m_z < minVal)
     435                        minVal = m_floats[1];
     436                }
     437                if (m_floats[2] < minVal)
    385438                {
    386439                        minIndex = 2;
    387                         minVal = m_z;
    388                 }
    389                 if (m_unusedW < minVal)
     440                        minVal = m_floats[2];
     441                }
     442                if (m_floats[3] < minVal)
    390443                {
    391444                        minIndex = 3;
    392                         minVal = m_unusedW;
     445                        minVal = m_floats[3];
    393446                }
    394447               

  • code/branches/physics/src/bullet/VERSION

    r2192 r2430  
    1 Bullet Collision Detection and Physics Library version 2.72
    2 http://bullet.googlecode.com
     12.73
Note: See TracChangeset for help on using the changeset viewer.