diff options
| author | Erwin Coumans <blender@erwincoumans.com> | 2008-09-13 11:06:43 +0400 |
|---|---|---|
| committer | Erwin Coumans <blender@erwincoumans.com> | 2008-09-13 11:06:43 +0400 |
| commit | 7f293488d12b5d5076b4bbf3d6c9248867c447a0 (patch) | |
| tree | 977ac9f1063de48615e8f294bfbcadb2a3b645f6 /extern/bullet2/src/BulletCollision | |
| parent | 206cfe7955683ac166201e417977e933fd98f7b3 (diff) | |
Upgrade to latest Bullet trunk, that is in sync with Blender/extern/bullet2. (except for one define 'WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER')
In case someone reads those SVN logs: you can enable some extra broadphase SSE optimizations by replacing WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER by WIN32 in extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
Thanks to Benoit Bolsee for the upstream patch/contribution.
Removed some obsolete files, they were just intended for comparison/testing.
Diffstat (limited to 'extern/bullet2/src/BulletCollision')
28 files changed, 983 insertions, 418 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index e7c5fb5b6cf..d0ad09a385a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -27,6 +27,7 @@ #include "btOverlappingPairCallback.h" //#define DEBUG_BROADPHASE 1 +#define USE_OVERLAP_TEST_ON_REMOVES 1 /// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase. /// It uses quantized integers to represent the begin and end points for each of the 3 axis. @@ -52,9 +53,7 @@ public: }; public: - //This breaks the Intel compiler, see http://softwarecommunity.intel.com/isn/Community/en-US/forums/thread/30253577.aspx - class Handle : public btBroadphaseProxy - //ATTRIBUTE_ALIGNED16(class) Handle : public btBroadphaseProxy + class Handle : public btBroadphaseProxy { public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -80,7 +79,7 @@ protected: BP_FP_INT_TYPE m_numHandles; // number of active handles BP_FP_INT_TYPE m_maxHandles; // max number of handles Handle* m_pHandles; // handles pool - void* m_pHandlesRawPtr; + BP_FP_INT_TYPE m_firstFreeHandle; // free handles list Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) @@ -100,7 +99,7 @@ protected: void freeHandle(BP_FP_INT_TYPE handle); - bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB); + bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); #ifdef DEBUG_BROADPHASE void debugPrintAxis(int axis,bool checkCardinality=true); @@ -273,10 +272,9 @@ m_invalidPair(0) m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; - // allocate handles buffer and put all handles on free list - m_pHandlesRawPtr = btAlignedAlloc(sizeof(Handle)*maxHandles,16); - m_pHandles = new(m_pHandlesRawPtr) Handle[maxHandles]; - + // allocate handles buffer, using btAlignedAlloc, and put all handles on free list + m_pHandles = new Handle[maxHandles]; + m_maxHandles = maxHandles; m_numHandles = 0; @@ -327,7 +325,7 @@ btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal() { btAlignedFree(m_pEdgesRawPtr[i]); } - btAlignedFree(m_pHandlesRawPtr); + delete [] m_pHandles; if (m_ownsPairCache) { @@ -603,34 +601,17 @@ bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* pr } template <typename BP_FP_INT_TYPE> -bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB) +bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1) { //optimization 1: check the array index (memory address), instead of the m_pos - for (int axis = 0; axis < 3; axis++) + if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || + pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] || + pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] || + pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) { - if (axis != ignoreAxis) - { - if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || - pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) - { - return false; - } - } + return false; } - - //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization) - - /*for (int axis = 0; axis < 3; axis++) - { - if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos || - m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos) - { - return false; - } - } - */ - return true; } @@ -700,7 +681,9 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE if (pPrev->IsMax()) { // if previous edge is a maximum check the bounds and add an overlap if necessary - if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev)) + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) { m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); if (m_userPairCallback) @@ -748,12 +731,19 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE ed if (pNext->IsMax()) { - + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + // if next edge is maximum remove any overlap between the two handles - if (updateOverlaps) + if (updateOverlaps +#ifdef USE_OVERLAP_TEST_ON_REMOVES + && testOverlap2D(handle0,handle1,axis1,axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) { - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pNext->m_handle); + m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); if (m_userPairCallback) @@ -799,12 +789,20 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE if (!pPrev->IsMax()) { // if previous edge was a minimum remove any overlap between the two handles - if (updateOverlaps) + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pPrev->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + if (updateOverlaps +#ifdef USE_OVERLAP_TEST_ON_REMOVES + && testOverlap2D(handle0,handle1,axis1,axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) { //this is done during the overlappingpairarray iteration/narrowphase collision - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pPrev->m_handle); + m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); if (m_userPairCallback) m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); @@ -850,10 +848,13 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE ed { Handle* pHandleNext = getHandle(pNext->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + if (!pNext->IsMax()) { // if next edge is a minimum check the bounds and add an overlap if necessary - if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext)) + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2)) { Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pNext->m_handle); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index fade71179e6..7c41c8d8f71 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -663,24 +663,23 @@ Benchmarking dbvt... Leaves: 8192 sizeof(btDbvtVolume): 32 bytes sizeof(btDbvtNode): 44 bytes -[1] btDbvtVolume intersections: 3537 ms (0%) -[2] btDbvtVolume merges: 1945 ms (0%) -[3] btDbvt::collideTT: 6646 ms (0%) -[4] btDbvt::collideTT self: 3389 ms (0%) -[5] btDbvt::collideTT xform: 7505 ms (0%) -[6] btDbvt::collideTT xform,self: 7480 ms (0%) -[7] btDbvt::collideRAY: 6307 ms (0%),(332511 r/s) -[8] insert/remove: 2105 ms (-3%),(996271 ir/s) -[9] updates (teleport): 1943 ms (0%),(1079337 u/s) -[10] updates (jitter): 1301 ms (0%),(1611953 u/s) -[11] optimize (incremental): 2510 ms (0%),(1671000 o/s) -[12] btDbvtVolume notequal: 3677 ms (0%) -[13] culling(OCL+fullsort): 2231 ms (0%),(458 t/s) -[14] culling(OCL+qsort): 3500 ms (0%),(2340 t/s) -[15] culling(KDOP+qsort): 1151 ms (0%),(7117 t/s) -[16] insert/remove batch(256): 5138 ms (0%),(816330 bir/s) -[17] btDbvtVolume proximity: 2842 ms (0%) -[18] btDbvtVolume select: 3390 ms (0%) +[1] btDbvtVolume intersections: 3499 ms (-1%) +[2] btDbvtVolume merges: 1934 ms (0%) +[3] btDbvt::collideTT: 5485 ms (-21%) +[4] btDbvt::collideTT self: 2814 ms (-20%) +[5] btDbvt::collideTT xform: 7379 ms (-1%) +[6] btDbvt::collideTT xform,self: 7270 ms (-2%) +[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s) +[8] insert/remove: 2093 ms (0%),(1001983 ir/s) +[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) +[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) +[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) +[12] btDbvtVolume notequal: 3659 ms (0%) +[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) +[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) +[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) +[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) +[17] btDbvtVolume select: 3419 ms (0%) */ struct btDbvtBenchmark @@ -787,7 +786,7 @@ static const bool cfgEnable = true; //[1] btDbvtVolume intersections bool cfgBenchmark1_Enable = cfgEnable; static const int cfgBenchmark1_Iterations = 8; -static const int cfgBenchmark1_Reference = 3537; +static const int cfgBenchmark1_Reference = 3499; //[2] btDbvtVolume merges bool cfgBenchmark2_Enable = cfgEnable; static const int cfgBenchmark2_Iterations = 4; @@ -795,21 +794,21 @@ static const int cfgBenchmark2_Reference = 1945; //[3] btDbvt::collideTT bool cfgBenchmark3_Enable = cfgEnable; static const int cfgBenchmark3_Iterations = 512; -static const int cfgBenchmark3_Reference = 6646; +static const int cfgBenchmark3_Reference = 5485; //[4] btDbvt::collideTT self bool cfgBenchmark4_Enable = cfgEnable; static const int cfgBenchmark4_Iterations = 512; -static const int cfgBenchmark4_Reference = 3389; +static const int cfgBenchmark4_Reference = 2814; //[5] btDbvt::collideTT xform bool cfgBenchmark5_Enable = cfgEnable; static const int cfgBenchmark5_Iterations = 512; static const btScalar cfgBenchmark5_OffsetScale = 2; -static const int cfgBenchmark5_Reference = 7505; +static const int cfgBenchmark5_Reference = 7379; //[6] btDbvt::collideTT xform,self bool cfgBenchmark6_Enable = cfgEnable; static const int cfgBenchmark6_Iterations = 512; static const btScalar cfgBenchmark6_OffsetScale = 2; -static const int cfgBenchmark6_Reference = 7480; +static const int cfgBenchmark6_Reference = 7270; //[7] btDbvt::collideRAY bool cfgBenchmark7_Enable = cfgEnable; static const int cfgBenchmark7_Passes = 32; @@ -824,13 +823,13 @@ static const int cfgBenchmark8_Reference = 2105; bool cfgBenchmark9_Enable = cfgEnable; static const int cfgBenchmark9_Passes = 32; static const int cfgBenchmark9_Iterations = 65536; -static const int cfgBenchmark9_Reference = 1943; +static const int cfgBenchmark9_Reference = 1879; //[10] updates (jitter) bool cfgBenchmark10_Enable = cfgEnable; static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000; static const int cfgBenchmark10_Passes = 32; static const int cfgBenchmark10_Iterations = 65536; -static const int cfgBenchmark10_Reference = 1301; +static const int cfgBenchmark10_Reference = 1244; //[11] optimize (incremental) bool cfgBenchmark11_Enable = cfgEnable; static const int cfgBenchmark11_Passes = 64; @@ -857,14 +856,10 @@ bool cfgBenchmark16_Enable = cfgEnable; static const int cfgBenchmark16_BatchCount = 256; static const int cfgBenchmark16_Passes = 16384; static const int cfgBenchmark16_Reference = 5138; -//[17] proximity +//[17] select bool cfgBenchmark17_Enable = cfgEnable; -static const int cfgBenchmark17_Iterations = 8; -static const int cfgBenchmark17_Reference = 2842; -//[18] select -bool cfgBenchmark18_Enable = cfgEnable; -static const int cfgBenchmark18_Iterations = 4; -static const int cfgBenchmark18_Reference = 3390; +static const int cfgBenchmark17_Iterations = 4; +static const int cfgBenchmark17_Reference = 3390; btClock wallclock; printf("Benchmarking dbvt...\r\n"); @@ -1259,32 +1254,6 @@ if(cfgBenchmark17_Enable) {// Benchmark 17 srand(380843); btAlignedObjectArray<btDbvtVolume> volumes; - btAlignedObjectArray<btScalar> results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i<cfgLeaves;++i) - { - volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); - } - printf("[17] btDbvtVolume proximity: "); - wallclock.reset(); - for(int i=0;i<cfgBenchmark17_Iterations;++i) - { - for(int j=0;j<cfgLeaves;++j) - { - for(int k=0;k<cfgLeaves;++k) - { - results[k]=Proximity(volumes[j],volumes[k]); - } - } - } - const int time=(int)wallclock.getTimeMilliseconds(); - printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time); - } -if(cfgBenchmark18_Enable) - {// Benchmark 18 - srand(380843); - btAlignedObjectArray<btDbvtVolume> volumes; btAlignedObjectArray<int> results; btAlignedObjectArray<int> indices; volumes.resize(cfgLeaves); @@ -1299,9 +1268,9 @@ if(cfgBenchmark18_Enable) { btSwap(indices[i],indices[rand()%cfgLeaves]); } - printf("[18] btDbvtVolume select: "); + printf("[17] btDbvtVolume select: "); wallclock.reset(); - for(int i=0;i<cfgBenchmark18_Iterations;++i) + for(int i=0;i<cfgBenchmark17_Iterations;++i) { for(int j=0;j<cfgLeaves;++j) { @@ -1313,7 +1282,7 @@ if(cfgBenchmark18_Enable) } } const int time=(int)wallclock.getTimeMilliseconds(); - printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark18_Reference)*100/time); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time); } printf("\r\n\r\n"); } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h index 10f94627c37..da296445e81 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -31,7 +31,7 @@ subject to the following restrictions: #define DBVT_IMPL_SSE 1 // SSE // Template implementation of ICollide -#ifdef WIN32_AVOID_WHEN_EMBEDDED_INSIDE_BLENDER +#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds #if (defined (_MSC_VER) && _MSC_VER >= 1400) #define DBVT_USE_TEMPLATE 1 #else @@ -41,6 +41,9 @@ subject to the following restrictions: #define DBVT_USE_TEMPLATE 0 #endif +// Use only intrinsics instead of inline asm +#define DBVT_USE_INTRINSIC_SSE 1 + // Using memmov for collideOCL #define DBVT_USE_MEMMOVE 1 @@ -57,14 +60,21 @@ subject to the following restrictions: #endif // Specific methods implementation -#ifdef WIN32_AVOID_WHEN_EMBEDDED_INSIDE_BLENDER -#define DBVT_PROXIMITY_IMPL DBVT_IMPL_SSE + +#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds #define DBVT_SELECT_IMPL DBVT_IMPL_SSE #define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE #else -#define DBVT_PROXIMITY_IMPL DBVT_IMPL_GENERIC #define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC #define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#endif + +#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#include <emmintrin.h> #endif // @@ -104,10 +114,6 @@ subject to the following restrictions: #error "DBVT_ENABLE_BENCHMARK undefined" #endif -#ifndef DBVT_PROXIMITY_IMPL -#error "DBVT_PROXIMITY_IMPL undefined" -#endif - #ifndef DBVT_SELECT_IMPL #error "DBVT_SELECT_IMPL undefined" #endif @@ -116,6 +122,10 @@ subject to the following restrictions: #error "DBVT_MERGE_IMPL undefined" #endif +#ifndef DBVT_INT0_IMPL +#error "DBVT_INT0_IMPL undefined" +#endif + // // Defaults volumes // @@ -133,8 +143,8 @@ static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); -DBVT_INLINE void Expand(const btVector3 e); -DBVT_INLINE void SignedExpand(const btVector3 e); +DBVT_INLINE void Expand(const btVector3& e); +DBVT_INLINE void SignedExpand(const btVector3& e); DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; @@ -173,12 +183,12 @@ struct btDbvtNode { btDbvtVolume volume; btDbvtNode* parent; - bool isleaf() const { return(childs[1]==0); } - bool isinternal() const { return(!isleaf()); } + DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + DBVT_INLINE bool isinternal() const { return(!isleaf()); } union { - btDbvtNode* childs[2]; - void* data; - }; + btDbvtNode* childs[2]; + void* data; + }; }; ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). @@ -186,8 +196,6 @@ struct btDbvtNode ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. struct btDbvt { - - /* Stack element */ struct sStkNN { @@ -250,8 +258,8 @@ struct btDbvt }; // Fields - btDbvtNode* m_root; - btDbvtNode* m_free; + btDbvtNode* m_root; + btDbvtNode* m_free; int m_lkhd; int m_leaves; unsigned m_opath; @@ -408,17 +416,17 @@ return(box); } // -DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3 e) +DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) { mi-=e;mx+=e; } // -DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3 e) +DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) { -if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x()); -if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y()); -if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z()); +if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); +if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); +if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); } // @@ -486,12 +494,19 @@ for(int i=0;i<3;++i) DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, const btDbvtAabbMm& b) { +#if DBVT_INT0_IMPL == DBVT_IMPL_SSE +const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); +const __int32* pu((const __int32*)&rt); +return((pu[0]|pu[1]|pu[2])==0); +#else return( (a.mi.x()<=b.mx.x())&& (a.mx.x()>=b.mi.x())&& (a.mi.y()<=b.mx.y())&& (a.mx.y()>=b.mi.y())&& (a.mi.z()<=b.mx.z())&& (a.mx.z()>=b.mi.z())); +#endif } // @@ -558,32 +573,8 @@ return(txmax>0); DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, const btDbvtAabbMm& b) { -#if DBVT_PROXIMITY_IMPL == DBVT_IMPL_SSE -DBVT_ALIGN btScalar r[1]; -static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; -__asm - { - mov eax,a - mov ecx,b - movaps xmm0,[eax] - movaps xmm2,[ecx] - movaps xmm1,[eax+16] - movaps xmm3,[ecx+16] - addps xmm0,xmm1 - addps xmm2,xmm3 - subps xmm0,xmm2 - andps xmm0,mask - movhlps xmm1,xmm0 - addps xmm0,xmm1 - pshufd xmm1,xmm0,1 - addss xmm0,xmm1 - movss r,xmm0 - } -return(r[0]); -#else const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); -#endif } // @@ -592,36 +583,57 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o, const btDbvtAabbMm& b) { #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE -DBVT_ALIGN __int32 r[1]; static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; -__asm - { - mov eax,o - mov ecx,a - mov edx,b - movaps xmm0,[eax] - movaps xmm5,mask - addps xmm0,[eax+16] - movaps xmm1,[ecx] - movaps xmm2,[edx] - addps xmm1,[ecx+16] - addps xmm2,[edx+16] - subps xmm1,xmm0 - subps xmm2,xmm0 - andps xmm1,xmm5 - andps xmm2,xmm5 - movhlps xmm3,xmm1 - movhlps xmm4,xmm2 - addps xmm1,xmm3 - addps xmm2,xmm4 - pshufd xmm3,xmm1,1 - pshufd xmm4,xmm2,1 - addss xmm1,xmm3 - addss xmm2,xmm4 - cmpless xmm2,xmm1 - movss r,xmm2 - } -return(r[0]&1); + // TODO: the intrinsic version is 11% slower + #if DBVT_USE_INTRINSIC_SSE + __m128 omi(_mm_load_ps(o.mi)); + omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); + ami=_mm_sub_ps(ami,omi); + ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); + bmi=_mm_sub_ps(bmi,omi); + bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami,ami)); + ami=_mm_add_ps(ami,t0); + ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); + bmi=_mm_add_ps(bmi,t1); + bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); + return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); + #else + DBVT_ALIGN __int32 r[1]; + __asm + { + mov eax,o + mov ecx,a + mov edx,b + movaps xmm0,[eax] + movaps xmm5,mask + addps xmm0,[eax+16] + movaps xmm1,[ecx] + movaps xmm2,[edx] + addps xmm1,[ecx+16] + addps xmm2,[edx+16] + subps xmm1,xmm0 + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } + return(r[0]&1); + #endif #else return(Proximity(o,a)<Proximity(o,b)?0:1); #endif @@ -633,20 +645,14 @@ DBVT_INLINE void Merge( const btDbvtAabbMm& a, btDbvtAabbMm& r) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE -__asm - { - mov eax,a - mov edx,b - mov ecx,r - movaps xmm0,[eax+0] - movaps xmm1,[edx+0] - movaps xmm2,[eax+16] - movaps xmm3,[edx+16] - minps xmm0,xmm1 - maxps xmm2,xmm3 - movaps [ecx+0],xmm0 - movaps [ecx+16],xmm2 - } +__m128 ami(_mm_load_ps(a.mi)); +__m128 amx(_mm_load_ps(a.mx)); +__m128 bmi(_mm_load_ps(b.mi)); +__m128 bmx(_mm_load_ps(b.mx)); +ami=_mm_min_ps(ami,bmi); +amx=_mm_max_ps(amx,bmx); +_mm_store_ps(r.mi,ami); +_mm_store_ps(r.mx,amx); #else for(int i=0;i<3;++i) { @@ -717,7 +723,7 @@ if(root0&&root1) int treshold=DOUBLE_STACKSIZE-4; stack.resize(DOUBLE_STACKSIZE); stack[0]=sStkNN(root0,root1); - do { + do { sStkNN p=stack[--depth]; if(depth>treshold) { @@ -838,12 +844,13 @@ collideTT(root0,root1,xform,policy); // DBVT_PREFIX inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, + const btDbvtVolume& vol, DBVT_IPOLICY) { DBVT_CHECKTYPE if(root) { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); btAlignedObjectArray<const btDbvtNode*> stack; stack.reserve(SIMPLE_STACKSIZE); stack.push_back(root); @@ -1095,7 +1102,10 @@ if(root) #undef DBVT_IPOLICY #undef DBVT_CHECKTYPE #undef DBVT_IMPL_GENERIC -#undef DBVT_IMPL_FPU0x86 #undef DBVT_IMPL_SSE +#undef DBVT_USE_INTRINSIC_SSE +#undef DBVT_SELECT_IMPL +#undef DBVT_MERGE_IMPL +#undef DBVT_INT0_IMPL #endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 2d27f22567f..a57952ffa06 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -55,17 +55,15 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o m_maxHandles = maxProxies; m_numHandles = 0; m_firstFreeHandle = 0; - m_firstAllocatedHandle = -1; + { for (int i = m_firstFreeHandle; i < maxProxies; i++) { m_pHandles[i].SetNextFree(i + 1); m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes - m_pHandles[i].SetNextAllocated(-1); } m_pHandles[maxProxies - 1].SetNextFree(0); - m_pHandles[maxProxies - 1].SetNextAllocated(-1); } @@ -179,31 +177,29 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) //first check for new overlapping pairs int i,j; - if (m_firstAllocatedHandle >= 0) + if (m_numHandles >= 0) { - btSimpleBroadphaseProxy* proxy0 = &m_pHandles[m_firstAllocatedHandle]; - for (i=0;i<m_numHandles;i++) { - btSimpleBroadphaseProxy* proxy1 = &m_pHandles[m_firstAllocatedHandle]; + btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i]; - for (j=0;j<m_numHandles;j++) + for (j=i+1;j<m_numHandles;j++) { - - if (proxy0 != proxy1) - { - btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); - btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); + btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; + btAssert(proxy0 != proxy1); - if (aabbOverlap(p0,p1)) - { - if ( !m_pairCache->findPair(proxy0,proxy1)) - { - m_pairCache->addOverlappingPair(proxy0,proxy1); - } - } else + btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); + btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); + + if (aabbOverlap(p0,p1)) + { + if ( !m_pairCache->findPair(proxy0,proxy1)) { + m_pairCache->addOverlappingPair(proxy0,proxy1); + } + } else + { if (!m_pairCache->hasDeferredRemoval()) { if ( m_pairCache->findPair(proxy0,proxy1)) @@ -211,19 +207,13 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher); } } - - } } - proxy1 = &m_pHandles[proxy1->GetNextAllocated()]; - } - proxy0 = &m_pHandles[proxy0->GetNextAllocated()]; - } if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) { - + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end @@ -237,11 +227,11 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) previousPair.m_pProxy0 = 0; previousPair.m_pProxy1 = 0; previousPair.m_algorithm = 0; - - + + for (i=0;i<overlappingPairArray.size();i++) { - + btBroadphasePair& pair = overlappingPairArray[i]; bool isDuplicate = (pair == previousPair); @@ -268,31 +258,31 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) //should have no algorithm btAssert(!pair.m_algorithm); } - + if (needsRemoval) { m_pairCache->cleanOverlappingPair(pair,dispatcher); - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); pair.m_pProxy0 = 0; pair.m_pProxy1 = 0; m_invalidPair++; gOverlappingPairs--; } - + } - ///if you don't like to skip the invalid pairs in the array, execute following code: - #define CLEAN_INVALID_PAIRS 1 - #ifdef CLEAN_INVALID_PAIRS + ///if you don't like to skip the invalid pairs in the array, execute following code: +#define CLEAN_INVALID_PAIRS 1 +#ifdef CLEAN_INVALID_PAIRS //perform a sort, to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS +#endif//CLEAN_INVALID_PAIRS } } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 49dfeb84900..e2ebb825725 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -25,7 +25,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy btVector3 m_min; btVector3 m_max; int m_nextFree; - int m_nextAllocated; + // int m_handleId; @@ -42,8 +42,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;} SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;} - SIMD_FORCE_INLINE void SetNextAllocated(int next) {m_nextAllocated = next;} - SIMD_FORCE_INLINE int GetNextAllocated() const {return m_nextAllocated;} + }; @@ -57,22 +56,18 @@ protected: int m_numHandles; // number of active handles int m_maxHandles; // max number of handles + btSimpleBroadphaseProxy* m_pHandles; // handles pool + void* m_pHandlesRawPtr; int m_firstFreeHandle; // free handles list - int m_firstAllocatedHandle; - + int allocHandle() { - + btAssert(m_numHandles < m_maxHandles); int freeHandle = m_firstFreeHandle; m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree(); - - m_pHandles[freeHandle].SetNextAllocated(m_firstAllocatedHandle); - m_firstAllocatedHandle = freeHandle; - m_numHandles++; - return freeHandle; } @@ -84,13 +79,9 @@ protected: proxy->SetNextFree(m_firstFreeHandle); m_firstFreeHandle = handle; - m_firstAllocatedHandle = proxy->GetNextAllocated(); - proxy->SetNextAllocated(-1); - m_numHandles--; } - btOverlappingPairCache* m_pairCache; bool m_ownsPairCache; diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt index e565bf7edea..d77ca6444c7 100644 --- a/extern/bullet2/src/BulletCollision/CMakeLists.txt +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -5,56 +5,149 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src } ADD_LIBRARY(LibBulletCollision BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btAxisSweep3.h BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btBroadphaseProxy.h BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btCollisionAlgorithm.h BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btDispatcher.h + BroadphaseCollision/btDbvtBroadphase.cpp + BroadphaseCollision/btDbvtBroadphase.h + BroadphaseCollision/btDbvt.cpp + BroadphaseCollision/btDbvt.h + BroadphaseCollision/btMultiSapBroadphase.cpp + BroadphaseCollision/btMultiSapBroadphase.h BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btOverlappingPairCache.h + BroadphaseCollision/btOverlappingPairCallback.h + BroadphaseCollision/btQuantizedBvh.cpp + BroadphaseCollision/btQuantizedBvh.h BroadphaseCollision/btSimpleBroadphase.cpp + BroadphaseCollision/btSimpleBroadphase.h CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionDispatcher.h CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionObject.h CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCollisionWorld.h CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.h CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + CollisionDispatch/btDefaultCollisionConfiguration.cpp + CollisionDispatch/btDefaultCollisionConfiguration.h CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.h + CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxCollisionAlgorithm.h + CollisionDispatch/btBoxBoxDetector.cpp + CollisionDispatch/btBoxBoxDetector.h CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.h + CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + CollisionDispatch/btConvexPlaneCollisionAlgorithm.h + CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp + CollisionDispatch/btSphereTriangleCollisionAlgorithm.h CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.h CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.h CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btManifoldResult.h CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btSimulationIslandManager.h CollisionDispatch/btUnionFind.cpp + CollisionDispatch/btUnionFind.h + CollisionDispatch/SphereTriangleDetector.cpp + CollisionDispatch/SphereTriangleDetector.h CollisionShapes/btBoxShape.cpp + CollisionShapes/btBoxShape.h CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btBvhTriangleMeshShape.h + CollisionShapes/btCapsuleShape.cpp + CollisionShapes/btCapsuleShape.h CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCollisionShape.h CollisionShapes/btCompoundShape.cpp + CollisionShapes/btCompoundShape.h CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConcaveShape.h CollisionShapes/btConeShape.cpp + CollisionShapes/btConeShape.h CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexHullShape.h CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvexShape.h + CollisionShapes/btConvexInternalShape.cpp + CollisionShapes/btConvexInternalShape.h CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btConvexTriangleMeshShape.h CollisionShapes/btCylinderShape.cpp + CollisionShapes/btCylinderShape.h CollisionShapes/btEmptyShape.cpp + CollisionShapes/btEmptyShape.h + CollisionShapes/btHeightfieldTerrainShape.cpp + CollisionShapes/btHeightfieldTerrainShape.h CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMinkowskiSumShape.h + CollisionShapes/btMaterial.h + CollisionShapes/btMultimaterialTriangleMeshShape.cpp + CollisionShapes/btMultimaterialTriangleMeshShape.h CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btMultiSphereShape.h CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btOptimizedBvh.h CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btPolyhedralConvexShape.h + CollisionShapes/btScaledBvhTriangleMeshShape.cpp + CollisionShapes/btScaledBvhTriangleMeshShape.h CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btTetrahedronShape.h CollisionShapes/btSphereShape.cpp + CollisionShapes/btSphereShape.h + CollisionShapes/btShapeHull.h + CollisionShapes/btShapeHull.cpp CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStaticPlaneShape.h CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btStridingMeshInterface.h CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleCallback.h CollisionShapes/btTriangleBuffer.cpp + CollisionShapes/btTriangleBuffer.h CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleIndexVertexArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.cpp CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMesh.h CollisionShapes/btTriangleMeshShape.cpp + CollisionShapes/btTriangleMeshShape.h + CollisionShapes/btUniformScalingShape.cpp + CollisionShapes/btUniformScalingShape.h NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btContinuousConvexCollision.h NarrowPhaseCollision/btGjkEpa.cpp + NarrowPhaseCollision/btGjkEpa.h + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpa2.h NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btConvexCast.h NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.h NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btGjkPairDetector.h NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btPersistentManifold.h NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btRaycastCallback.h NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.h NarrowPhaseCollision/btVoronoiSimplexSolver.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.h ) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index e55604632c0..45ebff5dc45 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -226,7 +226,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) a = btScalar(j)*(2*M__PI/m) + A[i0]; if (a > M__PI) a -= 2*M__PI; btScalar maxdiff=1e9,diff; -#ifndef dNODEBUG +#if defined(DEBUG) || defined (_DEBUG) *iret = i0; // iret is not allowed to keep this value #endif for (i=0; i<n; i++) { @@ -239,7 +239,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) } } } -#ifndef dNODEBUG +#if defined(DEBUG) || defined (_DEBUG) btAssert (*iret != i0); // ensure iret got set #endif avail[*iret] = 0; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index d1641249568..eebd0c99fcb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -31,7 +31,7 @@ btCollisionObject::btCollisionObject() m_internalType(CO_COLLISION_OBJECT), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdSquareMotionThreshold(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), m_checkCollideWith(false) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 96cbd1c3eb8..eb297ef4a3b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -81,8 +81,8 @@ protected: ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: btScalar m_ccdSweptSphereRadius; - /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - btScalar m_ccdSquareMotionThreshold; + /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold + btScalar m_ccdMotionThreshold; /// If some object should have elaborate collision filtering by sub-classes bool m_checkCollideWith; @@ -332,16 +332,22 @@ public: m_ccdSweptSphereRadius = radius; } + btScalar getCcdMotionThreshold() const + { + return m_ccdMotionThreshold; + } + btScalar getCcdSquareMotionThreshold() const { - return m_ccdSquareMotionThreshold; + return m_ccdMotionThreshold*m_ccdMotionThreshold; } - /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - void setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold) + + /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold + void setCcdMotionThreshold(btScalar ccdMotionThreshold) { - m_ccdSquareMotionThreshold = ccdSquareMotionThreshold; + m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 2fd972a4761..d8674a320a7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -127,6 +127,11 @@ void btCollisionWorld::updateAabbs() { btPoint3 minAabb,maxAabb; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + //need to increase the aabb for contact thresholds + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not @@ -381,14 +386,14 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btTransform childWorldTrans = colObjWorldTransform * childTrans; // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); - collisionObject->setCollisionShape((btCollisionShape*)childCollisionShape); + collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); rayTestSingle(rayFromTrans,rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback); // restore - collisionObject->setCollisionShape(saveCollisionShape); + collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } } } @@ -577,14 +582,14 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt btTransform childWorldTrans = colObjWorldTransform * childTrans; // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); - collisionObject->setCollisionShape((btCollisionShape*)childCollisionShape); + collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration); // restore - collisionObject->setCollisionShape(saveCollisionShape); + collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index a29a6d624e5..da9336c55c7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -16,12 +16,17 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" - +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) :btCollisionAlgorithm(ci), -m_isSwapped(isSwapped) +m_isSwapped(isSwapped), +m_sharedManifold(ci.m_manifold) { + m_ownsManifold = false; + btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; assert (colObj->getCollisionShape()->isCompound()); @@ -33,11 +38,17 @@ m_isSwapped(isSwapped) m_childCollisionAlgorithms.resize(numChildren); for (i=0;i<numChildren;i++) { - btCollisionShape* tmpShape = colObj->getCollisionShape(); - btCollisionShape* childShape = compoundShape->getChildShape(i); - colObj->internalSetTemporaryCollisionShape( childShape ); - m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj); - colObj->internalSetTemporaryCollisionShape( tmpShape ); + if (compoundShape->getDynamicAabbTree()) + { + m_childCollisionAlgorithms[i] = 0; + } else + { + btCollisionShape* tmpShape = colObj->getCollisionShape(); + btCollisionShape* childShape = compoundShape->getChildShape(i); + colObj->internalSetTemporaryCollisionShape( childShape ); + m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold); + colObj->internalSetTemporaryCollisionShape( tmpShape ); + } } } @@ -48,11 +59,109 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() int i; for (i=0;i<numChildren;i++) { - m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + if (m_childCollisionAlgorithms[i]) + { + m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + } } } + + + +struct btCompoundLeafCallback : btDbvt::ICollide +{ + +public: + + btCollisionObject* m_compoundColObj; + btCollisionObject* m_otherObj; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + btCollisionAlgorithm** m_childCollisionAlgorithms; + btPersistentManifold* m_sharedManifold; + + + + + btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) + :m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithms(childCollisionAlgorithms), + m_sharedManifold(sharedManifold) + { + + } + + + void ProcessChildShape(btCollisionShape* childShape,int index) + { + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape()); + + + //backup + btTransform orgTrans = m_compoundColObj->getWorldTransform(); + btTransform orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(index); + btTransform newChildWorldTrans = orgTrans*childTrans ; + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1); + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + + m_compoundColObj->setWorldTransform( newChildWorldTrans); + m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans); + + //the contactpoint is still projected back using the original inverted worldtrans + btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape(); + m_compoundColObj->internalSetTemporaryCollisionShape( childShape ); + + if (!m_childCollisionAlgorithms[index]) + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold); + + m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut); + if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 worldAabbMin,worldAabbMax; + m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1)); + m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1)); + } + + //revert back transform + m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape); + m_compoundColObj->setWorldTransform( orgTrans ); + m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans); + } + } + void Process(const btDbvtNode* leaf) + { + int index = int(leaf->data); + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape()); + btCollisionShape* childShape = compoundShape->getChildShape(index); + if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 worldAabbMin,worldAabbMax; + btTransform orgTrans = m_compoundColObj->getWorldTransform(); + btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax); + m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0)); + } + ProcessChildShape(childShape,index); + + } +}; + + + + + + void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { btCollisionObject* colObj = m_isSwapped? body1 : body0; @@ -61,37 +170,69 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt assert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); - //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps - //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals - //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: - //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 - //then use each overlapping node AABB against Tree0 - //and vise versa. + btDbvt* tree = compoundShape->getDynamicAabbTree(); + //use a dynamic aabb tree to cull potential child-overlaps + btCompoundLeafCallback callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); - int numChildren = m_childCollisionAlgorithms.size(); - int i; - for (i=0;i<numChildren;i++) + + if (tree) { - //temporarily exchange parent btCollisionShape with childShape, and recurse - btCollisionShape* childShape = compoundShape->getChildShape(i); - //backup - btTransform orgTrans = colObj->getWorldTransform(); - btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform(); + btVector3 localAabbMin,localAabbMax; + btTransform otherInCompoundSpace; + otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform(); + otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); - const btTransform& childTrans = compoundShape->getChildTransform(i); - btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( newChildWorldTrans); - colObj->setInterpolationWorldTransform(newChildWorldTrans); + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + //process all children, that overlap with the given AABB bounds + tree->collideTV(tree->m_root,bounds,callback); + + } else + { + //iterate over all children, perform an AABB check inside ProcessChildShape + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + callback.ProcessChildShape(compoundShape->getChildShape(i),i); + } + } + + { + //iterate over all children, perform an AABB check inside ProcessChildShape + int numChildren = m_childCollisionAlgorithms.size(); + int i; + btManifoldArray manifoldArray; + + for (i=0;i<numChildren;i++) + { + if (m_childCollisionAlgorithms[i]) + { + btCollisionShape* childShape = compoundShape->getChildShape(i); + //if not longer overlapping, remove the algorithm + btTransform orgTrans = colObj->getWorldTransform(); + btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(i); + btTransform newChildWorldTrans = orgTrans*childTrans ; + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1); + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + m_childCollisionAlgorithms[i] = 0; + } + + } + + } + + - //the contactpoint is still projected back using the original inverted worldtrans - btCollisionShape* tmpShape = colObj->getCollisionShape(); - colObj->internalSetTemporaryCollisionShape( childShape ); - m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut); - //revert back - colObj->internalSetTemporaryCollisionShape( tmpShape); - colObj->setWorldTransform( orgTrans ); - colObj->setInterpolationWorldTransform(orgInterpolationTrans); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 1682c6761cd..624a3cf10f5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -28,11 +28,13 @@ class btDispatcher; class btDispatcher; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -/// Place holder, not fully implemented yet class btCompoundCollisionAlgorithm : public btCollisionAlgorithm { btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; + + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; public: @@ -49,7 +51,8 @@ public: int i; for (i=0;i<m_childCollisionAlgorithms.size();i++) { - m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); + if (m_childCollisionAlgorithms[i]) + m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 3f94f4d4eac..1c317080544 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -22,7 +22,9 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" @@ -70,11 +72,14 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; m_boxSphereCF->m_swapped = true; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); @@ -176,10 +181,13 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_sphereSphereCF); +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_sphereBoxCF); m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_sphereTriangleCF); m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); @@ -212,7 +220,7 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg { return m_sphereSphereCF; } - +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) { return m_sphereBoxCF; @@ -222,6 +230,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg { return m_boxSphereCF; } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 0b274d394af..4f5af5f048c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -71,8 +71,11 @@ class btDefaultCollisionConfiguration : public btCollisionConfiguration btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; btCollisionAlgorithmCreateFunc* m_sphereSphereCF; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM btCollisionAlgorithmCreateFunc* m_sphereBoxCF; btCollisionAlgorithmCreateFunc* m_boxSphereCF; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + btCollisionAlgorithmCreateFunc* m_boxBoxCF; btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; btCollisionAlgorithmCreateFunc* m_triangleSphereCF; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 46961f5d61b..fdbd4abef27 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -63,7 +63,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co input.m_transformA = sphereObj->getWorldTransform(); input.m_transformB = triObj->getWorldTransform(); - bool swapResults = m_swapped && !m_ownManifold; + bool swapResults = m_swapped; detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index adac455bbcb..8bc2cfa6ffa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -21,19 +21,7 @@ subject to the following restrictions: void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - btMatrix3x3 abs_b = t.getBasis().absolute(); - btPoint3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents), - abs_b[1].dot(halfExtents), - abs_b[2].dot(halfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); - - aabbMin = center - extent; - aabbMax = center + extent; - - + btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 92bcce55119..d4b046d40c9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -49,10 +49,11 @@ public: { btVector3 halfExtents(getRadius(),getRadius(),getRadius()); halfExtents[m_upAxis] = getRadius() + getHalfHeight(); + halfExtents += btVector3(getMargin(),getMargin(),getMargin()); btMatrix3x3 abs_b = t.getBasis().absolute(); btPoint3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); + aabbMin = center - extent; aabbMax = center + extent; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 740f1783630..f08b810eadb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -14,23 +14,29 @@ subject to the following restrictions: */ #include "btCompoundShape.h" - - #include "btCollisionShape.h" - +#include "BulletCollision/BroadphaseCollision/btDbvt.h" btCompoundShape::btCompoundShape() :m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), -m_aabbTree(0), m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), +m_dynamicAabbTree(0) { + void* mem = btAlignedAlloc(sizeof(btDbvt),16); + m_dynamicAabbTree = new(mem) btDbvt(); + btAssert(mem==m_dynamicAabbTree); } btCompoundShape::~btCompoundShape() { + if (m_dynamicAabbTree) + { + m_dynamicAabbTree->~btDbvt(); + btAlignedFree(m_dynamicAabbTree); + } } void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) @@ -60,71 +66,88 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio } } + if (m_dynamicAabbTree) + { + const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + int index = m_children.size()-1; + child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); + } + +} + +void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) +{ + btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); + if (m_dynamicAabbTree) + { + m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); + } + m_children.swap(childShapeIndex,m_children.size()-1); + m_children.pop_back(); + } void btCompoundShape::removeChildShape(btCollisionShape* shape) { - bool done_removing; - - // Find the children containing the shape specified, and remove those children. - do - { - done_removing = true; - - for(int i = 0; i < m_children.size(); i++) - { - if(m_children[i].m_childShape == shape) - { - m_children.remove(m_children[i]); - done_removing = false; // Do another iteration pass after removing from the vector - break; - } - } - } - while (!done_removing); - - recalculateLocalAabb(); + // Find the children containing the shape specified, and remove those children. + //note: there might be multiple children using the same shape! + for(int i = m_children.size()-1; i >= 0 ; i--) + { + if(m_children[i].m_childShape == shape) + { + m_children.swap(i,m_children.size()-1); + m_children.pop_back(); + //remove it from the m_dynamicAabbTree too + //m_dynamicAabbTree->remove(m_aabbProxies[i]); + //m_aabbProxies.swap(i,m_children.size()-1); + //m_aabbProxies.pop_back(); + } + } + + + + recalculateLocalAabb(); } void btCompoundShape::recalculateLocalAabb() { - // Recalculate the local aabb - // Brute force, it iterates over all the shapes left. - m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - - //extend the local aabbMin/aabbMax - for (int j = 0; j < m_children.size(); j++) - { - btVector3 localAabbMin,localAabbMax; - m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); - for (int i=0;i<3;i++) - { - if (m_localAabbMin[i] > localAabbMin[i]) - m_localAabbMin[i] = localAabbMin[i]; - if (m_localAabbMax[i] < localAabbMax[i]) - m_localAabbMax[i] = localAabbMax[i]; - } - } + // Recalculate the local aabb + // Brute force, it iterates over all the shapes left. + m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + + //extend the local aabbMin/aabbMax + for (int j = 0; j < m_children.size(); j++) + { + btVector3 localAabbMin,localAabbMax; + m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); + for (int i=0;i<3;i++) + { + if (m_localAabbMin[i] > localAabbMin[i]) + m_localAabbMin[i] = localAabbMin[i]; + if (m_localAabbMax[i] < localAabbMax[i]) + m_localAabbMax[i] = localAabbMax[i]; + } + } } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - + btMatrix3x3 abs_b = trans.getBasis().absolute(); btPoint3 center = trans(localCenter); btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), - abs_b[1].dot(localHalfExtents), - abs_b[2].dot(localHalfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMin = center-extent; + aabbMax = center+extent; - aabbMin = center - extent; - aabbMax = center + extent; } void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const @@ -134,9 +157,9 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co ident.setIdentity(); btVector3 aabbMin,aabbMax; getAabb(ident,aabbMin,aabbMax); - + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - + btScalar lx=btScalar(2.)*(halfExtents.x()); btScalar ly=btScalar(2.)*(halfExtents.y()); btScalar lz=btScalar(2.)*(halfExtents.z()); @@ -147,5 +170,62 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co } - - + + + +void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +{ + int n = m_children.size(); + + btScalar totalMass = 0; + btVector3 center(0, 0, 0); + for (int k = 0; k < n; k++) + { + center += m_children[k].m_transform.getOrigin() * masses[k]; + totalMass += masses[k]; + } + center /= totalMass; + principal.setOrigin(center); + + btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); + for (int k = 0; k < n; k++) + { + btVector3 i; + m_children[k].m_childShape->calculateLocalInertia(masses[k], i); + + const btTransform& t = m_children[k].m_transform; + btVector3 o = t.getOrigin() - center; + + //compute inertia tensor in coordinate system of compound shape + btMatrix3x3 j = t.getBasis().transpose(); + j[0] *= i[0]; + j[1] *= i[1]; + j[2] *= i[2]; + j = t.getBasis() * j; + + //add inertia tensor + tensor[0] += j[0]; + tensor[1] += j[1]; + tensor[2] += j[2]; + + //compute inertia tensor of pointmass at o + btScalar o2 = o.length2(); + j[0].setValue(o2, 0, 0); + j[1].setValue(0, o2, 0); + j[2].setValue(0, 0, o2); + j[0] += o * -o.x(); + j[1] += o * -o.y(); + j[2] += o * -o.z(); + + //add inertia tensor of pointmass + tensor[0] += masses[k] * j[0]; + tensor[1] += masses[k] * j[1]; + tensor[2] += masses[k] * j[2]; + } + + tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); +} + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index f77e4b59558..3624749f768 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -24,8 +24,8 @@ subject to the following restrictions: #include "btCollisionMargin.h" #include "LinearMath/btAlignedObjectArray.h" -class btOptimizedBvh; - +//class btOptimizedBvh; +struct btDbvt; ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild { @@ -35,14 +35,15 @@ ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild btCollisionShape* m_childShape; int m_childShapeType; btScalar m_childMargin; + struct btDbvtNode* m_node; }; SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2) { - return ( c1.m_transform == c2.m_transform && - c1.m_childShape == c2.m_childShape && - c1.m_childShapeType == c2.m_childShapeType && - c1.m_childMargin == c2.m_childMargin ); + return ( c1.m_transform == c2.m_transform && + c1.m_childShape == c2.m_childShape && + c1.m_childShapeType == c2.m_childShapeType && + c1.m_childMargin == c2.m_childMargin ); } /// btCompoundShape allows to store multiple other btCollisionShapes @@ -55,7 +56,8 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape btVector3 m_localAabbMin; btVector3 m_localAabbMax; - btOptimizedBvh* m_aabbTree; + //btOptimizedBvh* m_aabbTree; + btDbvt* m_dynamicAabbTree; public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -66,11 +68,11 @@ public: void addChildShape(const btTransform& localTransform,btCollisionShape* shape); - /** Remove all children shapes that contain the specified shape. */ + /// Remove all children shapes that contain the specified shape virtual void removeChildShape(btCollisionShape* shape); - - + void removeChildShapeByIndex(int childShapeindex); + int getNumChildShapes() const { @@ -103,9 +105,9 @@ public: ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. - Use this yourself if you modify the children or their transforms. */ + + /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. + Use this yourself if you modify the children or their transforms. */ virtual void recalculateLocalAabb(); virtual void setLocalScaling(const btVector3& scaling) @@ -118,7 +120,7 @@ public: } virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - + virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} virtual void setMargin(btScalar margin) @@ -137,11 +139,19 @@ public: //this is optional, but should make collision queries faster, by culling non-overlapping nodes void createAabbTreeFromChildren(); - const btOptimizedBvh* getAabbTree() const + btDbvt* getDynamicAabbTree() { - return m_aabbTree; + return m_dynamicAabbTree; } + ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia + ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform + ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound + ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform + ///of the collision object by the principal transform. + void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + + private: btScalar m_collisionMargin; protected: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index f1682aaf65b..02a293e82ba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -204,3 +204,113 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const { return m_stridingMesh->getScaling(); } + +void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const +{ + class CenterCallback: public btInternalTriangleIndexCallback + { + bool first; + btVector3 ref; + btVector3 sum; + btScalar volume; + + public: + + CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void) triangleIndex; + (void) partId; + if (first) + { + ref = triangle[0]; + first = false; + } + else + { + btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); + sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); + volume += vol; + } + } + + btVector3 getCenter() + { + return (volume > 0) ? sum / volume : ref; + } + + btScalar getVolume() + { + return volume * btScalar(1. / 6); + } + + }; + + class InertiaCallback: public btInternalTriangleIndexCallback + { + btMatrix3x3 sum; + btVector3 center; + + public: + + InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void) triangleIndex; + (void) partId; + btMatrix3x3 i; + btVector3 a = triangle[0] - center; + btVector3 b = triangle[1] - center; + btVector3 c = triangle[2] - center; + btVector3 abc = a + b + c; + btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); + for (int j = 0; j < 3; j++) + { + for (int k = 0; k <= j; k++) + { + i[j][k] = i[k][j] = volNeg * (center[j] * center[k] + + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j]) + + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + + 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])); + } + } + btScalar i00 = -i[0][0]; + btScalar i11 = -i[1][1]; + btScalar i22 = -i[2][2]; + i[0][0] = i11 + i22; + i[1][1] = i22 + i00; + i[2][2] = i00 + i11; + sum[0] += i[0]; + sum[1] += i[1]; + sum[2] += i[2]; + } + + btMatrix3x3& getInertia() + { + return sum; + } + + }; + + CenterCallback centerCallback; + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); + btVector3 center = centerCallback.getCenter(); + principal.setOrigin(center); + volume = centerCallback.getVolume(); + + InertiaCallback inertiaCallback(center); + m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); + + btMatrix3x3& i = inertiaCallback.getInertia(); + i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(i[0][0], i[1][1], i[2][2]); + inertia /= volume; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index 01eecf27811..6ff0bf5d43b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -46,6 +46,13 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia + ///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia" + ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the + ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform + ///of the collision object by the principal transform. This method also computes the volume of the convex mesh. + void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const; + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 9e4800a46fe..a291d6b7ce7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -91,19 +91,15 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { -/* - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); -*/ - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + halfExtents += btVector3(getMargin(),getMargin(),getMargin()); btMatrix3x3 abs_b = t.getBasis().absolute(); btPoint3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents), abs_b[1].dot(halfExtents), abs_b[2].dot(halfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); + aabbMin = center - extent; aabbMax = center + extent; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 9b78fc0f7c2..a248c55b571 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -306,6 +306,8 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f if (curNodeSubPart >= 0) meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); + + curNodeSubPart = nodeSubPart; btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 0d55e4fe92c..4c4ce7feaa7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -18,6 +18,7 @@ subject to the following restrictions: #include "LinearMath/btPoint3.h" #include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAabbUtil2.h" #include "btConvexInternalShape.h" @@ -46,28 +47,7 @@ public: //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - - btAssert(m_localAabbMin.getX() <= m_localAabbMax.getX()); - btAssert(m_localAabbMin.getY() <= m_localAabbMax.getY()); - btAssert(m_localAabbMin.getZ() <= m_localAabbMax.getZ()); - - - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); - - btPoint3 center = trans(localCenter); - - btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), - abs_b[1].dot(localHalfExtents), - abs_b[2].dot(localHalfExtents)); - extent += btVector3(margin,margin,margin); - - aabbMin = center - extent; - aabbMax = center + extent; - - + btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp new file mode 100644 index 00000000000..5a17b4e2df0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -0,0 +1,103 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btScaledBvhTriangleMeshShape.h" + +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling) +:m_bvhTriMeshShape(childShape), +m_localScaling(localScaling) +{ + +} + +btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() +{ +} + + +class btScaledTriangleCallback : public btTriangleCallback +{ + btTriangleCallback* m_originalCallback; + + btVector3 m_localScaling; + +public: + + btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling) + :m_originalCallback(originalCallback), + m_localScaling(localScaling) + { + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 newTriangle[3]; + newTriangle[0] = triangle[0]*m_localScaling; + newTriangle[1] = triangle[1]*m_localScaling; + newTriangle[2] = triangle[2]*m_localScaling; + m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); + } +}; + +void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + btScaledTriangleCallback scaledCallback(callback,m_localScaling); + + btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); + btVector3 scaledAabbMin = aabbMin * invLocalScaling; + btVector3 scaledAabbMax = aabbMax * invLocalScaling; + m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); +} + + +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); + btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); + localAabbMin *= m_localScaling; + localAabbMax *= m_localScaling; + + btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + btScalar margin = m_bvhTriMeshShape->getMargin(); + localHalfExtents += btVector3(margin,margin,margin); + btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMin = center - extent; + aabbMax = center + extent; + +} + +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} + +const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const +{ + return m_localScaling; +} + +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + ///don't make this a movable object! + btAssert(0); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h new file mode 100644 index 00000000000..8e1d29fc005 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -0,0 +1,57 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" + + +///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. +///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' +ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape +{ + + + btVector3 m_localScaling; + + btBvhTriangleMeshShape* m_bvhTriMeshShape; + +public: + + + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling); + + virtual ~btScaledBvhTriangleMeshShape(); + + virtual int getShapeType() const + { + //use un-used 'FAST_CONCAVE_MESH_PROXYTYPE' for now, later add SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE to btBroadphaseProxy.h + return FAST_CONCAVE_MESH_PROXYTYPE; + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + //debugging + virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + +}; + +#endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index cd0a00bbd6f..0a2c77096d1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -47,6 +47,7 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt { btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); btMatrix3x3 abs_b = trans.getBasis().absolute(); @@ -56,12 +57,10 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), abs_b[2].dot(localHalfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); - aabbMin = center - extent; aabbMax = center + extent; - + } void btTriangleMeshShape::recalcLocalAabb() diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 9fba77cb547..c9eabafe290 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -65,6 +65,16 @@ public: return m_meshInterface; } + const btVector3& getLocalAabbMin() const + { + return m_localAabbMin; + } + const btVector3& getLocalAabbMax() const + { + return m_localAabbMax; + } + + //debugging virtual const char* getName()const {return "TRIANGLEMESH";} |