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 | |
| 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')
57 files changed, 1302 insertions, 3470 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";} diff --git a/extern/bullet2/src/BulletDynamics/CMakeLists.txt b/extern/bullet2/src/BulletDynamics/CMakeLists.txt index 8598575799a..58b023e9775 100644 --- a/extern/bullet2/src/BulletDynamics/CMakeLists.txt +++ b/extern/bullet2/src/BulletDynamics/CMakeLists.txt @@ -5,16 +5,32 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src } ADD_LIBRARY(LibBulletDynamics ConstraintSolver/btContactConstraint.cpp + ConstraintSolver/btContactConstraint.h + ConstraintSolver/btConeTwistConstraint.cpp + ConstraintSolver/btConeTwistConstraint.h ConstraintSolver/btGeneric6DofConstraint.cpp + ConstraintSolver/btGeneric6DofConstraint.h ConstraintSolver/btHingeConstraint.cpp + ConstraintSolver/btHingeConstraint.h ConstraintSolver/btPoint2PointConstraint.cpp + ConstraintSolver/btPoint2PointConstraint.h ConstraintSolver/btSequentialImpulseConstraintSolver.cpp + ConstraintSolver/btSequentialImpulseConstraintSolver.h + ConstraintSolver/btSliderConstraint.cpp + ConstraintSolver/btSliderConstraint.h ConstraintSolver/btSolve2LinearConstraint.cpp + ConstraintSolver/btSolve2LinearConstraint.h ConstraintSolver/btTypedConstraint.cpp + ConstraintSolver/btTypedConstraint.h + Dynamics/Bullet-C-API.cpp Dynamics/btDiscreteDynamicsWorld.cpp + Dynamics/btDiscreteDynamicsWorld.h Dynamics/btSimpleDynamicsWorld.cpp - Dynamics/Bullet-C-API.cpp + Dynamics/btSimpleDynamicsWorld.h Dynamics/btRigidBody.cpp + Dynamics/btRigidBody.h Vehicle/btRaycastVehicle.cpp + Vehicle/btRaycastVehicle.h Vehicle/btWheelInfo.cpp + Vehicle/btWheelInfo.h ) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index e11b49d9420..61dad522a5b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -114,17 +114,34 @@ void btConeTwistConstraint::buildJacobian() btScalar swing1=btScalar(0.),swing2 = btScalar(0.); + btScalar swx=btScalar(0.),swy = btScalar(0.); + btScalar thresh = btScalar(10.); + btScalar fact; + // Get Frame into world space if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); - swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); +// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis2); + swing1 = btAtan2Fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + btScalar(1.0)); + swing1 *= fact; + } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); - swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); +// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis3); + swing2 = btAtan2Fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + btScalar(1.0)); + swing2 *= fact; } btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 114abce24c7..a0523a8c76b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -241,15 +241,18 @@ void btHingeConstraint::buildJacobian() m_solveLimit = false; m_accLimitImpulse = btScalar(0.); - if (m_lowerLimit < m_upperLimit) +// if (m_lowerLimit < m_upperLimit) + if (m_lowerLimit <= m_upperLimit) { - if (hingeAngle <= m_lowerLimit*m_limitSoftness) +// if (hingeAngle <= m_lowerLimit*m_limitSoftness) + if (hingeAngle <= m_lowerLimit) { m_correction = (m_lowerLimit - hingeAngle); m_limitSign = 1.0f; m_solveLimit = true; } - else if (hingeAngle >= m_upperLimit*m_limitSoftness) +// else if (hingeAngle >= m_upperLimit*m_limitSoftness) + else if (hingeAngle >= m_upperLimit) { m_correction = m_upperLimit - hingeAngle; m_limitSign = -1.0f; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp deleted file mode 100644 index 7d2f19998ac..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 "btOdeContactJoint.h" -#include "btOdeSolverBody.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - - -//this constant needs to be set up so different solvers give 'similar' results -#define FRICTION_CONSTANT 120.f - - -btOdeContactJoint::btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1) -:m_manifold(manifold), -m_index(index), -m_swapBodies(swap), -m_body0(body0), -m_body1(body1) -{ -} - -int m_numRows = 3; - - -void btOdeContactJoint::GetInfo1(Info1 *info) -{ - info->m = m_numRows; - //friction adds another 2... - - info->nub = 0; -} - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - -#define M_SQRT12 btScalar(0.7071067811865475244008443621048490) - -#define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ - - - -void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q); -void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q) -{ - if (btFabs(n[2]) > M_SQRT12) { - // choose p in y-z plane - btScalar a = n[1]*n[1] + n[2]*n[2]; - btScalar k = dRecipSqrt (a); - p[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; - // set q = n x p - q[0] = a*k; - q[1] = -n[0]*p[2]; - q[2] = n[0]*p[1]; - } - else { - // choose p in x-y plane - btScalar a = n[0]*n[0] + n[1]*n[1]; - btScalar k = dRecipSqrt (a); - p[0] = -n[1]*k; - p[1] = n[0]*k; - p[2] = 0; - // set q = n x p - q[0] = -n[2]*p[1]; - q[1] = n[2]*p[0]; - q[2] = a*k; - } -} - - - -void btOdeContactJoint::GetInfo2(Info2 *info) -{ - - int s = info->rowskip; - int s2 = 2*s; - - float swapFactor = m_swapBodies ? -1.f : 1.f; - - // get normal, with sign adjusted for body1/body2 polarity - dVector3 normal; - - - btManifoldPoint& point = m_manifold->getContactPoint(m_index); - - normal[0] = swapFactor*point.m_normalWorldOnB.x(); - normal[1] = swapFactor*point.m_normalWorldOnB.y(); - normal[2] = swapFactor*point.m_normalWorldOnB.z(); - normal[3] = 0; // @@@ hmmm - - assert(m_body0); - // if (GetBody0()) - btVector3 relativePositionA; - { - relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition; - dVector3 c1; - c1[0] = relativePositionA.x(); - c1[1] = relativePositionA.y(); - c1[2] = relativePositionA.z(); - - // set jacobian for normal - info->J1l[0] = normal[0]; - info->J1l[1] = normal[1]; - info->J1l[2] = normal[2]; - dCROSS (info->J1a,=,c1,normal); - - } - - btVector3 relativePositionB(0,0,0); - if (m_body1) - { - // if (GetBody1()) - - { - dVector3 c2; - btVector3 posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0); - relativePositionB = point.getPositionWorldOnB() - posBody1; - - // for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - - // j->node[1].body->pos[i]; - c2[0] = relativePositionB.x(); - c2[1] = relativePositionB.y(); - c2[2] = relativePositionB.z(); - - info->J2l[0] = -normal[0]; - info->J2l[1] = -normal[1]; - info->J2l[2] = -normal[2]; - dCROSS (info->J2a,= -,c2,normal); - } - } - - btScalar k = info->fps * info->erp; - - float depth = -point.getDistance(); -// if (depth < 0.f) -// depth = 0.f; - - info->c[0] = k * depth; - //float maxvel = .2f; - -// if (info->c[0] > maxvel) -// info->c[0] = maxvel; - - - //can override it, not necessary -// info->cfm[0] = 0.f; -// info->cfm[1] = 0.f; -// info->cfm[2] = 0.f; - - - - // set LCP limits for normal - info->lo[0] = 0; - info->hi[0] = 1e30f;//dInfinity; - info->lo[1] = 0; - info->hi[1] = 0.f; - info->lo[2] = 0.f; - info->hi[2] = 0.f; - -#define DO_THE_FRICTION_2 -#ifdef DO_THE_FRICTION_2 - // now do jacobian for tangential forces - dVector3 t1,t2; // two vectors tangential to normal - - dVector3 c1; - c1[0] = relativePositionA.x(); - c1[1] = relativePositionA.y(); - c1[2] = relativePositionA.z(); - - dVector3 c2; - c2[0] = relativePositionB.x(); - c2[1] = relativePositionB.y(); - c2[2] = relativePositionB.z(); - - //combined friction is available in the contact point - float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction; - - // first friction direction - if (m_numRows >= 2) - { - - - - dPlaneSpace1 (normal,t1,t2); - - info->J1l[s+0] = t1[0]; - info->J1l[s+1] = t1[1]; - info->J1l[s+2] = t1[2]; - dCROSS (info->J1a+s,=,c1,t1); -// if (1) { //j->node[1].body) { - info->J2l[s+0] = -t1[0]; - info->J2l[s+1] = -t1[1]; - info->J2l[s+2] = -t1[2]; - dCROSS (info->J2a+s,= -,c2,t1); -// } - // set right hand side -// if (0) {//j->contact.surface.mode & dContactMotion1) { - //info->c[1] = j->contact.surface.motion1; -// } - // set LCP bounds and friction index. this depends on the approximation - // mode - //1e30f - - - info->lo[1] = -friction;//-j->contact.surface.mu; - info->hi[1] = friction;//j->contact.surface.mu; -// if (1)//j->contact.surface.mode & dContactApprox1_1) - info->findex[1] = 0; - - // set slip (constraint force mixing) -// if (0)//j->contact.surface.mode & dContactSlip1) -// { -// // info->cfm[1] = j->contact.surface.slip1; -// } else -// { -// //info->cfm[1] = 0.f; -// } - } - - // second friction direction - if (m_numRows >= 3) { - info->J1l[s2+0] = t2[0]; - info->J1l[s2+1] = t2[1]; - info->J1l[s2+2] = t2[2]; - dCROSS (info->J1a+s2,=,c1,t2); -// if (1) { //j->node[1].body) { - info->J2l[s2+0] = -t2[0]; - info->J2l[s2+1] = -t2[1]; - info->J2l[s2+2] = -t2[2]; - dCROSS (info->J2a+s2,= -,c2,t2); -// } - - // set right hand side -// if (0){//j->contact.surface.mode & dContactMotion2) { - //info->c[2] = j->contact.surface.motion2; -// } - // set LCP bounds and friction index. this depends on the approximation - // mode -// if (0){//j->contact.surface.mode & dContactMu2) { - //info->lo[2] = -j->contact.surface.mu2; - //info->hi[2] = j->contact.surface.mu2; -// } -// else { - info->lo[2] = -friction; - info->hi[2] = friction; -// } -// if (0)//j->contact.surface.mode & dContactApprox1_2) - -// { -// info->findex[2] = 0; -// } - // set slip (constraint force mixing) -// if (0) //j->contact.surface.mode & dContactSlip2) - -// { - //info->cfm[2] = j->contact.surface.slip2; - -// } - } - -#endif //DO_THE_FRICTION_2 - -} - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h deleted file mode 100644 index 8dd0282c59e..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 CONTACT_JOINT_H -#define CONTACT_JOINT_H - -#include "btOdeJoint.h" -struct btOdeSolverBody; -class btPersistentManifold; - -class btOdeContactJoint : public btOdeJoint -{ - btPersistentManifold* m_manifold; - int m_index; - bool m_swapBodies; - btOdeSolverBody* m_body0; - btOdeSolverBody* m_body1; - - -public: - - btOdeContactJoint() {}; - - btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1); - - //btOdeJoint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); - - - - -}; - -#endif //CONTACT_JOINT_H - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp deleted file mode 100644 index 46c3783c6a0..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 "btOdeJoint.h" - -btOdeJoint::btOdeJoint() -{ - -} -btOdeJoint::~btOdeJoint() -{ - -} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h deleted file mode 100644 index 50733d1418f..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h +++ /dev/null @@ -1,94 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 btOdeJoint_H -#define btOdeJoint_H - -struct btOdeSolverBody; -class btOdeJoint; - -#include "LinearMath/btScalar.h" - -struct BU_ContactJointNode { - btOdeJoint *joint; // pointer to enclosing btOdeJoint object - btOdeSolverBody* body; // *other* body this joint is connected to -}; -typedef btScalar dVector3[4]; - - -class btOdeJoint { - -public: - // naming convention: the "first" body this is connected to is node[0].body, - // and the "second" body is node[1].body. if this joint is only connected - // to one body then the second body is 0. - - // info returned by getInfo1 function. the constraint dimension is m (<=6). - // i.e. that is the total number of rows in the jacobian. `nub' is the - // number of unbounded variables (which have lo,hi = -/+ infinity). - - btOdeJoint(); - virtual ~btOdeJoint(); - - - struct Info1 { - int m,nub; - }; - - // info returned by getInfo2 function - - struct Info2 { - // integrator parameters: frames per second (1/stepsize), default error - // reduction parameter (0..1). - btScalar fps,erp; - - // for the first and second body, pointers to two (linear and angular) - // n*3 jacobian sub matrices, stored by rows. these matrices will have - // been initialized to 0 on entry. if the second body is zero then the - // J2xx pointers may be 0. - btScalar *J1l,*J1a,*J2l,*J2a; - - // elements to jump from one row to the next in J's - int rowskip; - - // right hand sides of the equation J*v = c + cfm * lambda. cfm is the - // "constraint force mixing" vector. c is set to zero on entry, cfm is - // set to a constant value (typically very small or zero) value on entry. - btScalar *c,*cfm; - - // lo and hi limits for variables (set to -/+ infinity on entry). - btScalar *lo,*hi; - - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; - }; - - // virtual function table: size of the joint structure, function pointers. - // we do it this way instead of using C++ virtual functions because - // sometimes we need to allocate joints ourself within a memory pool. - - virtual void GetInfo1 (Info1 *info)=0; - virtual void GetInfo2 (Info2 *info)=0; - - int flags; // dJOINT_xxx flags - BU_ContactJointNode node[2]; // connections to bodies. node[1].body can be 0 - btScalar lambda[6]; // lambda generated by last step -}; - - -#endif //btOdeJoint_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h deleted file mode 100644 index e4bc2628bd4..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h +++ /dev/null @@ -1,212 +0,0 @@ -/* - * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - Bullet Continuous Collision Detection and Physics Library - Bullet is Copyright (c) 2003-2006 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. -*/ - -#define ODE_MACROS -#ifdef ODE_MACROS - -#include "LinearMath/btScalar.h" - -typedef btScalar dVector4[4]; -typedef btScalar dMatrix3[4*3]; -#define dInfinity FLT_MAX - - - -#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ - - - -#define dMULTIPLY0_331NEW(A,op,B,C) \ -{\ - btScalar tmp[3];\ - tmp[0] = C.getX();\ - tmp[1] = C.getY();\ - tmp[2] = C.getZ();\ - dMULTIPLYOP0_331(A,op,B,tmp);\ -} - -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) -#define dMULTIPLYOP0_331(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B+4),(C)); \ - (A)[2] op dDOT1((B+8),(C)); - -#define dAASSERT btAssert -#define dIASSERT btAssert - -#define REAL float -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -inline btScalar dDOT1 (const btScalar *a, const btScalar *b) -{ return dDOTpq(a,b,1,1); } -#define dDOT14(a,b) dDOTpq(a,b,1,4) - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - -/* - * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. - * A is stored by rows, and has `skip' elements per row. the matrix is - * assumed to be already zero, so this does not write zero elements! - * if (plus,minus) is (+,-) then a positive version will be written. - * if (plus,minus) is (-,+) then a negative version will be written. - */ - -#define dCROSSMAT(A,a,skip,plus,minus) \ -{ \ - (A)[1] = minus (a)[2]; \ - (A)[2] = plus (a)[1]; \ - (A)[(skip)+0] = plus (a)[2]; \ - (A)[(skip)+2] = minus (a)[0]; \ - (A)[2*(skip)+0] = minus (a)[1]; \ - (A)[2*(skip)+1] = plus (a)[0]; \ -} - - -#define dMULTIPLYOP2_333(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B),(C+4)); \ - (A)[2] op dDOT1((B),(C+8)); \ - (A)[4] op dDOT1((B+4),(C)); \ - (A)[5] op dDOT1((B+4),(C+4)); \ - (A)[6] op dDOT1((B+4),(C+8)); \ - (A)[8] op dDOT1((B+8),(C)); \ - (A)[9] op dDOT1((B+8),(C+4)); \ - (A)[10] op dDOT1((B+8),(C+8)); - -#define dMULTIPLYOP0_333(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); \ - (A)[4] op dDOT14((B+4),(C)); \ - (A)[5] op dDOT14((B+4),(C+1)); \ - (A)[6] op dDOT14((B+4),(C+2)); \ - (A)[8] op dDOT14((B+8),(C)); \ - (A)[9] op dDOT14((B+8),(C+1)); \ - (A)[10] op dDOT14((B+8),(C+2)); - -#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) -#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) -#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) - - -//////////////////////////////////////////////////////////////////// -#define EFFICIENT_ALIGNMENT 16 -#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) -/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste - * up to 15 bytes per allocation, depending on what alloca() returns. - */ - -#define dALLOCA16(n) \ - ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) - -//#define ALLOCA dALLOCA16 - -typedef const btScalar *dRealPtr; -typedef btScalar *dRealMutablePtr; -//#define dRealArray(name,n) btScalar name[n]; -//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar)); - -/////////////////////////////////////////////////////////////////////////////// - - //Remotion: 10.10.2007 -#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) ); - -//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar)); -#define dRealAllocaArray(name,size) btScalar *name = NULL; \ - unsigned int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \ - if (memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \ - else{ btAssert(memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())); name = (btScalar*) alloca(memNeeded_##name); } - - - - - -/////////////////////////////////////////////////////////////////////////////// -#if 0 -inline void dSetZero1 (btScalar *a, int n) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = 0; - n--; - } -} - -inline void dSetValue1 (btScalar *a, int n, btScalar value) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = value; - n--; - } -} -#else - -/// This macros are for MSVC and XCode compilers. Remotion. - - -#include <string.h> //for memset - -//Remotion: 10.10.2007 -//------------------------------------------------------------------------------ -#define IS_ALIGNED_16(x) ((size_t(x)&15)==0) -//------------------------------------------------------------------------------ -inline void dSetZero1 (btScalar *dest, int size) -{ - dAASSERT (dest && size >= 0); - memset(dest, 0, size * sizeof(btScalar)); -} -//------------------------------------------------------------------------------ -inline void dSetValue1 (btScalar *dest, int size, btScalar val) -{ - dAASSERT (dest && size >= 0); - int n_mod4 = size & 3; - int n4 = size - n_mod4; -/*#ifdef __USE_SSE__ -//it is not supported on double precision, todo... - if(IS_ALIGNED_16(dest)){ - __m128 xmm0 = _mm_set_ps1(val); - for (int i=0; i<n4; i+=4) - { - _mm_store_ps(&dest[i],xmm0); - } - }else -#endif - */ - - { - for (int i=0; i<n4; i+=4) // Unrolled Loop - { - dest[i ] = val; - dest[i+1] = val; - dest[i+2] = val; - dest[i+3] = val; - } - } - for (int i=n4; i<size; i++){ - dest[i] = val; - } -} -#endif -///////////////////////////////////////////////////////////////////// - - -#endif //USE_SOR_SOLVER - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp deleted file mode 100644 index ab90c926559..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp +++ /dev/null @@ -1,393 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 "btOdeQuickstepConstraintSolver.h" - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "btOdeJoint.h" -#include "btOdeContactJoint.h" -#include "btOdeTypedJoint.h" -#include "btOdeSolverBody.h" -#include <new> -#include "LinearMath/btQuickprof.h" - -#include "LinearMath/btIDebugDraw.h" - -#define USE_SOR_SOLVER - -#include "btSorLcp.h" - -#include <math.h> -#include <float.h>//FLT_MAX -#ifdef WIN32 -#include <memory.h> -#endif -#include <string.h> -#include <stdio.h> - -#if defined (WIN32) -#include <malloc.h> -#else -#if defined (__FreeBSD__) -#include <stdlib.h> -#else -#include <alloca.h> -#endif -#endif - -class btOdeJoint; - -//see below -//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island - - -btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver(): - m_cfm(0.f),//1e-5f), - m_erp(0.4f) -{ -} - - -//iterative lcp and penalty method -btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** /*bodies*/,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) -{ - - m_CurBody = 0; - m_CurJoint = 0; - m_CurTypedJoint = 0; - int j; - - int max_contacts = 0; /// should be 4 //Remotion - for ( j=0;j<numManifolds;j++){ - btPersistentManifold* manifold = manifoldPtr[j]; - if (manifold->getNumContacts() > max_contacts) max_contacts = manifold->getNumContacts(); - } - //if(max_contacts > 4) - // printf(" max_contacts > 4"); - - int numBodies = 0; - m_odeBodies.clear(); - m_odeBodies.reserve(numBulletBodies + 1); //??? - // btOdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES]; - - int numJoints = 0; - m_joints.clear(); - m_joints.reserve(numManifolds * max_contacts + 4 + numConstraints + 1); //??? - // btOdeJoint* joints [ODE_MAX_SOLVER_JOINTS*2]; - - m_SolverBodyArray.resize(numBulletBodies + 1); - m_JointArray.resize(numManifolds * max_contacts + 4); - m_TypedJointArray.resize(numConstraints + 1); - - - //capture contacts - int body0=-1,body1=-1; - for (j=0;j<numManifolds;j++) - { - btPersistentManifold* manifold = manifoldPtr[j]; - if (manifold->getNumContacts() > 0) - { - body0 = ConvertBody((btRigidBody*)manifold->getBody0(),m_odeBodies,numBodies); - body1 = ConvertBody((btRigidBody*)manifold->getBody1(),m_odeBodies,numBodies); - ConvertConstraint(manifold,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer); - } - } - - //capture constraints - for (j=0;j<numConstraints;j++) - { - btTypedConstraint * typedconstraint = constraints[j]; - body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),m_odeBodies,numBodies); - body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),m_odeBodies,numBodies); - ConvertTypedConstraint(typedconstraint,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer); - } - //if(numBodies > numBulletBodies) - // printf(" numBodies > numBulletBodies"); - //if(numJoints > numManifolds * 4 + numConstraints) - // printf(" numJoints > numManifolds * 4 + numConstraints"); - - - m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do - - //write back resulting velocities - for (int i=0;i<numBodies;i++) - { - if (m_odeBodies[i]->m_invMass) - { - m_odeBodies[i]->m_originalBody->setLinearVelocity(m_odeBodies[i]->m_linearVelocity); - m_odeBodies[i]->m_originalBody->setAngularVelocity(m_odeBodies[i]->m_angularVelocity); - } - } - - - /// Remotion, just free all this here - m_odeBodies.clear(); - m_joints.clear(); - - m_SolverBodyArray.clear(); - m_JointArray.clear(); - m_TypedJointArray.clear(); - - return 0.f; - -} - -///////////////////////////////////////////////////////////////////////////////// - - -typedef btScalar dQuaternion[4]; -#define _R(i,j) R[(i)*4+(j)] - -void dRfromQ1 (dMatrix3 R, const dQuaternion q); -void dRfromQ1 (dMatrix3 R, const dQuaternion q) -{ - // q = (s,vx,vy,vz) - btScalar qq1 = 2.f*q[1]*q[1]; - btScalar qq2 = 2.f*q[2]*q[2]; - btScalar qq3 = 2.f*q[3]*q[3]; - _R(0,0) = 1.f - qq2 - qq3; - _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); - _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); - _R(0,3) = 0.f; - - _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1.f - qq1 - qq3; - _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); - _R(1,3) = 0.f; - - _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); - _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); - _R(2,2) = 1.f - qq1 - qq2; - _R(2,3) = 0.f; - -} - - - -//int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btOdeSolverBody** bodies,int& numBodies) -int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies) -{ - assert(orgBody); - if (!orgBody || (orgBody->getInvMass() == 0.f) ) - { - return -1; - } - - if (orgBody->getCompanionId()>=0) - { - return orgBody->getCompanionId(); - } - //first try to find - int i,j; - - //if not found, create a new body - // btOdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies]; - btOdeSolverBody* body = &m_SolverBodyArray[numBodies]; - bodies.push_back(body); // Remotion 10.10.07: - - orgBody->setCompanionId(numBodies); - - numBodies++; - - body->m_originalBody = orgBody; - - body->m_facc.setValue(0,0,0,0); - body->m_tacc.setValue(0,0,0,0); - - body->m_linearVelocity = orgBody->getLinearVelocity(); - body->m_angularVelocity = orgBody->getAngularVelocity(); - body->m_invMass = orgBody->getInvMass(); - body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition(); - body->m_friction = orgBody->getFriction(); - - //are the indices the same ? - for (i=0;i<4;i++) - { - for ( j=0;j<3;j++) - { - body->m_invI[i+4*j] = 0.f; - body->m_I[i+4*j] = 0.f; - } - } - body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x(); - body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y(); - body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z(); - - body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x(); - body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y(); - body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z(); - - - - - dQuaternion q; - - q[1] = orgBody->getOrientation().x(); - q[2] = orgBody->getOrientation().y(); - q[3] = orgBody->getOrientation().z(); - q[0] = orgBody->getOrientation().w(); - - dRfromQ1(body->m_R,q); - - return numBodies-1; -} - - - - - - - - - -void btOdeQuickstepConstraintSolver::ConvertConstraint(btPersistentManifold* manifold, - btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, - const btAlignedObjectArray< btOdeSolverBody*> &bodies, - int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) -{ - - - manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), - ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); - - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - - int i,numContacts = manifold->getNumContacts(); - - bool swapBodies = (bodyId0 < 0); - - - btOdeSolverBody* body0,*body1; - - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); - - } - else - { - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); - } - - assert(bodyId0 >= 0); - - btVector3 color(0,1,0); - for (i=0;i<numContacts;i++) - { - - //assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS); - -// if (manifold->getContactPoint(i).getDistance() < 0.0f) - { - - btOdeContactJoint* cont = new (&m_JointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1); - //btOdeContactJoint* cont = new (&gJointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1); - - cont->node[0].joint = cont; - cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; - - cont->node[1].joint = cont; - cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; - - // joints[numJoints++] = cont; - joints.push_back(cont); // Remotion 10.10.07: - numJoints++; - - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; - - cont->flags = 0; - } - } - - //create a new contact constraint -} - -void btOdeQuickstepConstraintSolver::ConvertTypedConstraint( - btTypedConstraint * constraint, - btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, - const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* /*debugDrawer*/) -{ - - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - bool swapBodies = (bodyId0 < 0); - - - btOdeSolverBody* body0,*body1; - - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); - - } - else - { - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); - } - - assert(bodyId0 >= 0); - - - //assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS); - - - btOdeTypedJoint * cont = NULL; - - // Determine constraint type - int joint_type = constraint->getConstraintType(); - switch(joint_type) - { - case POINT2POINT_CONSTRAINT_TYPE: - case D6_CONSTRAINT_TYPE: - cont = new (&m_TypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1); - //cont = new (&gTypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1); - break; - - }; - - if(cont) - { - cont->node[0].joint = cont; - cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; - - cont->node[1].joint = cont; - cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; - - // joints[numJoints++] = cont; - joints.push_back(cont); // Remotion 10.10.07: - numJoints++; - - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; - - cont->flags = 0; - } - -} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h deleted file mode 100644 index e548ea6fc22..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h +++ /dev/null @@ -1,109 +0,0 @@ -/* - * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - Bullet Continuous Collision Detection and Physics Library - Bullet is Copyright (c) 2003-2006 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 ODE_CONSTRAINT_SOLVER_H -#define ODE_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" - -#include "LinearMath/btAlignedObjectArray.h" -#include "btOdeContactJoint.h" -#include "btOdeTypedJoint.h" -#include "btOdeSolverBody.h" -#include "btSorLcp.h" - -class btRigidBody; -struct btOdeSolverBody; -class btOdeJoint; - -/// btOdeQuickstepConstraintSolver is one of the available solvers for Bullet dynamics framework -/// It uses an adapted version quickstep solver from the Open Dynamics Engine project -class btOdeQuickstepConstraintSolver : public btConstraintSolver -{ -private: - int m_CurBody; - int m_CurJoint; - int m_CurTypedJoint; - - float m_cfm; - float m_erp; - - btSorLcpSolver m_SorLcpSolver; - - btAlignedObjectArray<btOdeSolverBody*> m_odeBodies; - btAlignedObjectArray<btOdeJoint*> m_joints; - - btAlignedObjectArray<btOdeSolverBody> m_SolverBodyArray; - btAlignedObjectArray<btOdeContactJoint> m_JointArray; - btAlignedObjectArray<btOdeTypedJoint> m_TypedJointArray; - - -private: - int ConvertBody(btRigidBody* body,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies); - void ConvertConstraint(btPersistentManifold* manifold, - btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, - const btAlignedObjectArray< btOdeSolverBody*> &bodies, - int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); - - void ConvertTypedConstraint( - btTypedConstraint * constraint, - btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, - const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); - - -public: - - btOdeQuickstepConstraintSolver(); - - virtual ~btOdeQuickstepConstraintSolver() {} - - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); - - ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal - ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' - void setConstraintForceMixing(float cfm) { - m_cfm = cfm; - } - - ///setErrorReductionParamter sets the maximum amount of error reduction - ///which limits energy addition during penetration depth recovery - void setErrorReductionParamter(float erp) - { - m_erp = erp; - } - - ///clear internal cached data and reset random seed - void reset() - { - m_SorLcpSolver.dRand2_seed = 0; - } - - void setRandSeed(unsigned long seed) - { - m_SorLcpSolver.dRand2_seed = seed; - } - unsigned long getRandSeed() const - { - return m_SorLcpSolver.dRand2_seed; - } -}; - - - - -#endif //ODE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h deleted file mode 100644 index 0c936971b79..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h +++ /dev/null @@ -1,48 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 ODE_SOLVER_BODY_H -#define ODE_SOLVER_BODY_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -typedef btScalar dMatrix3[4*3]; - -///ODE's quickstep needs just a subset of the rigidbody data in its own layout, so make a temp copy -struct btOdeSolverBody -{ - btRigidBody* m_originalBody; - - btVector3 m_centerOfMassPosition; - /// for ode solver-binding - dMatrix3 m_R;//temp - dMatrix3 m_I; - dMatrix3 m_invI; - - int m_odeTag; - float m_invMass; - float m_friction; - - btVector3 m_tacc;//temp - btVector3 m_facc; - - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - -}; - - -#endif //#ifndef ODE_SOLVER_BODY_H - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp deleted file mode 100644 index f683bf7d748..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp +++ /dev/null @@ -1,880 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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 "btOdeTypedJoint.h" -#include "btOdeSolverBody.h" -#include "btOdeMacros.h" -#include <stdio.h> - -void btOdeTypedJoint::GetInfo1(Info1 *info) -{ - int joint_type = m_constraint->getConstraintType(); - switch (joint_type) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - p2pjoint.GetInfo1(info); - } - break; - case D6_CONSTRAINT_TYPE: - { - OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - d6joint.GetInfo1(info); - } - break; - case SLIDER_CONSTRAINT_TYPE: - { - OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - sliderjoint.GetInfo1(info); - } - break; - }; -} - -void btOdeTypedJoint::GetInfo2(Info2 *info) -{ - int joint_type = m_constraint->getConstraintType(); - switch (joint_type) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - p2pjoint.GetInfo2(info); - } - break; - case D6_CONSTRAINT_TYPE: - { - OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - d6joint.GetInfo2(info); - } - break; - case SLIDER_CONSTRAINT_TYPE: - { - OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - sliderjoint.GetInfo2(info); - } - break; - }; -} - - -OdeP2PJoint::OdeP2PJoint( - btTypedConstraint * constraint, - int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1): - btOdeTypedJoint(constraint,index,swap,body0,body1) -{ -} - - -void OdeP2PJoint::GetInfo1(Info1 *info) -{ - info->m = 3; - info->nub = 3; -} - - -void OdeP2PJoint::GetInfo2(Info2 *info) -{ - - btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint(); - - //retrieve matrices - btTransform body0_trans; - if (m_body0) - { - body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); - } -// btScalar body0_mat[12]; -// body0_mat[0] = body0_trans.getBasis()[0][0]; -// body0_mat[1] = body0_trans.getBasis()[0][1]; -// body0_mat[2] = body0_trans.getBasis()[0][2]; -// body0_mat[4] = body0_trans.getBasis()[1][0]; -// body0_mat[5] = body0_trans.getBasis()[1][1]; -// body0_mat[6] = body0_trans.getBasis()[1][2]; -// body0_mat[8] = body0_trans.getBasis()[2][0]; -// body0_mat[9] = body0_trans.getBasis()[2][1]; -// body0_mat[10] = body0_trans.getBasis()[2][2]; - - btTransform body1_trans; - - if (m_body1) - { - body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); - } -// btScalar body1_mat[12]; -// body1_mat[0] = body1_trans.getBasis()[0][0]; -// body1_mat[1] = body1_trans.getBasis()[0][1]; -// body1_mat[2] = body1_trans.getBasis()[0][2]; -// body1_mat[4] = body1_trans.getBasis()[1][0]; -// body1_mat[5] = body1_trans.getBasis()[1][1]; -// body1_mat[6] = body1_trans.getBasis()[1][2]; -// body1_mat[8] = body1_trans.getBasis()[2][0]; -// body1_mat[9] = body1_trans.getBasis()[2][1]; -// body1_mat[10] = body1_trans.getBasis()[2][2]; - - - - - // anchor points in global coordinates with respect to body PORs. - - - int s = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - - - btVector3 a1,a2; - - a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA(); - //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); - dCROSSMAT (info->J1a,a1,s,-,+); - if (m_body1) - { - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB(); - //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); - dCROSSMAT (info->J2a,a2,s,+,-); - } - - - // set right hand side - btScalar k = info->fps * info->erp; - if (m_body1) - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - - a1[j] - body0_trans.getOrigin()[j]); - } - } - else - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - - body0_trans.getOrigin()[j]); - } - } -} - - -///////////////////limit motor support - -/*! \pre testLimitValue must be called on limot*/ -int bt_get_limit_motor_info2( - btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational) -{ - - - int srow = row * info->rowskip; - - // if the joint is powered, or has joint limits, add in the extra row - int powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - - if (powered || limit) - { - btScalar *J1 = rotational ? info->J1a : info->J1l; - btScalar *J2 = rotational ? info->J2a : info->J2l; - - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if (body1) - { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - - // linear limot torque decoupling step: - // - // if this is a linear limot (e.g. from a slider), we have to be careful - // that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in powered or limited slider-jointed free - // bodies from gaining angular momentum. - // the solution used here is to apply the constraint forces at the point - // halfway between the body centers. there is no penalty (other than an - // extra tiny bit of computation) in doing this adjustment. note that we - // only need to do this if the constraint connects two bodies. - - btVector3 ltd; // Linear Torque Decoupling vector (a torque) - if (!rotational && body1) - { - btVector3 c; - c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0] - -body0->getCenterOfMassPosition()[0]); - c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1] - -body0->getCenterOfMassPosition()[1]); - c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2] - -body0->getCenterOfMassPosition()[2]); - - ltd = c.cross(ax1); - - info->J1a[srow+0] = ltd[0]; - info->J1a[srow+1] = ltd[1]; - info->J1a[srow+2] = ltd[2]; - info->J2a[srow+0] = ltd[0]; - info->J2a[srow+1] = ltd[1]; - info->J2a[srow+2] = ltd[2]; - } - - // if we're limited low and high simultaneously, the joint motor is - // ineffective - - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; - - if (powered) - { - info->cfm[row] = 0.0f;//limot->m_normalCFM; - if (! limit) - { - info->c[row] = limot->m_targetVelocity; - info->lo[row] = -limot->m_maxMotorForce; - info->hi[row] = limot->m_maxMotorForce; - } - } - - if (limit) - { - btScalar k = info->fps * limot->m_ERP; - info->c[row] = -k * limot->m_currentLimitError; - info->cfm[row] = 0.0f;//limot->m_stopCFM; - - if (limot->m_loLimit == limot->m_hiLimit) - { - // limited low and high simultaneously - info->lo[row] = -dInfinity; - info->hi[row] = dInfinity; - } - else - { - if (limit == 1) - { - // low limit - info->lo[row] = 0; - info->hi[row] = SIMD_INFINITY; - } - else - { - // high limit - info->lo[row] = -SIMD_INFINITY; - info->hi[row] = 0; - } - - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = body0->getAngularVelocity().dot(ax1); - if (body1) - vel -= body1->getAngularVelocity().dot(ax1); - } - else - { - vel = body0->getLinearVelocity().dot(ax1); - if (body1) - vel -= body1->getLinearVelocity().dot(ax1); - } - - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - // low limit - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->c[row]) info->c[row] = newc; - } - } - else - { - // high limit - all those computations are reversed - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->c[row]) info->c[row] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - - -///////////////////OdeD6Joint - - - - - -OdeD6Joint::OdeD6Joint( - btTypedConstraint * constraint, - int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1): - btOdeTypedJoint(constraint,index,swap,body0,body1) -{ -} - - -void OdeD6Joint::GetInfo1(Info1 *info) -{ - btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); - //prepare constraint - d6constraint->calculateTransforms(); - info->m = 3; - info->nub = 3; - - //test angular limits - for (int i=0;i<3 ;i++ ) - { - //if(i==2) continue; - if(d6constraint->testAngularLimitMotor(i)) - { - info->m++; - } - } - - -} - - -int OdeD6Joint::setLinearLimits(Info2 *info) -{ - - btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); - - //retrieve matrices - btTransform body0_trans; - if (m_body0) - { - body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); - } - - btTransform body1_trans; - - if (m_body1) - { - body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); - } - - // anchor points in global coordinates with respect to body PORs. - - int s = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - - - btVector3 a1,a2; - - a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); - //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); - dCROSSMAT (info->J1a,a1,s,-,+); - if (m_body1) - { - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); - - //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); - dCROSSMAT (info->J2a,a2,s,+,-); - } - - - // set right hand side - btScalar k = info->fps * info->erp; - if (m_body1) - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - - a1[j] - body0_trans.getOrigin()[j]); - } - } - else - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] - - body0_trans.getOrigin()[j]); - } - } - - return 3; - -} - -int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset) -{ - btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); - int row = row_offset; - //solve angular limits - for (int i=0;i<3 ;i++ ) - { - //if(i==2) continue; - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) - { - btVector3 axis = d6constraint->getAxis(i); - row += bt_get_limit_motor_info2( - d6constraint->getRotationalLimitMotor(i), - m_body0->m_originalBody, - m_body1 ? m_body1->m_originalBody : NULL, - info,row,axis,1); - } - } - - return row; -} - -void OdeD6Joint::GetInfo2(Info2 *info) -{ - int row = setLinearLimits(info); - setAngularLimits(info, row); -} - -//---------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------- -/* -OdeSliderJoint -Ported from ODE by Roman Ponomarev (rponom@gmail.com) -April 24, 2008 -*/ - -OdeSliderJoint::OdeSliderJoint( - btTypedConstraint * constraint, - int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1): - btOdeTypedJoint(constraint,index,swap,body0,body1) -{ -} // OdeSliderJoint::OdeSliderJoint() - -//---------------------------------------------------------------------------------- - -void OdeSliderJoint::GetInfo1(Info1* info) -{ - info->nub = 4; - info->m = 4; // Fixed 2 linear + 2 angular - btSliderConstraint * slider = this->getSliderConstraint(); - //prepare constraint - slider->calculateTransforms(); - slider->testLinLimits(); - if(slider->getSolveLinLimit() || slider->getPoweredLinMotor()) - { - info->m++; // limit 3rd linear as well - } - slider->testAngLimits(); - if(slider->getSolveAngLimit() || slider->getPoweredAngMotor()) - { - info->m++; // limit 3rd angular as well - } -} // OdeSliderJoint::GetInfo1() - -//---------------------------------------------------------------------------------- - -void OdeSliderJoint::GetInfo2(Info2 *info) -{ - int i, s = info->rowskip; - btSliderConstraint * slider = this->getSliderConstraint(); - const btTransform& trA = slider->getCalculatedTransformA(); - const btTransform& trB = slider->getCalculatedTransformB(); - // make rotations around Y and Z equal - // the slider axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the slider axis should be equal. thus the constraint equations are - // p*w1 - p*w2 = 0 - // q*w1 - q*w2 = 0 - // where p and q are unit vectors normal to the slider axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - // get slider axis (X) - btVector3 ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - // set the two slider rows - info->J1a[0] = p[0]; - info->J1a[1] = p[1]; - info->J1a[2] = p[2]; - info->J1a[s+0] = q[0]; - info->J1a[s+1] = q[1]; - info->J1a[s+2] = q[2]; - if(m_body1) - { - info->J2a[0] = -p[0]; - info->J2a[1] = -p[1]; - info->J2a[2] = -p[2]; - info->J2a[s+0] = -q[0]; - info->J2a[s+1] = -q[1]; - info->J2a[s+2] = -q[2]; - } - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the slider back into alignment. - // if ax1,ax2 are the unit length slider axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if "theta" is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. - btScalar k = info->fps * info->erp * slider->getSoftnessOrthoAng(); - btVector3 ax2 = trB.getBasis().getColumn(0); - btVector3 u; - if(m_body1) - { - u = ax1.cross(ax2); - } - else - { - u = ax2.cross(ax1); - } - info->c[0] = k * u.dot(p); - info->c[1] = k * u.dot(q); - // pull out pos and R for both bodies. also get the connection - // vector c = pos2-pos1. - // next two rows. we want: vel2 = vel1 + w1 x c ... but this would - // result in three equations, so we project along the planespace vectors - // so that sliding along the slider axis is disregarded. for symmetry we - // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. - btTransform bodyA_trans = m_body0->m_originalBody->getCenterOfMassTransform(); - btTransform bodyB_trans; - if(m_body1) - { - bodyB_trans = m_body1->m_originalBody->getCenterOfMassTransform(); - } - int s2 = 2 * s, s3 = 3 * s; - btVector3 c; - if(m_body1) - { - c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); - btVector3 tmp = btScalar(0.5) * c.cross(p); - - for (i=0; i<3; i++) info->J1a[s2+i] = tmp[i]; - for (i=0; i<3; i++) info->J2a[s2+i] = tmp[i]; - - tmp = btScalar(0.5) * c.cross(q); - - for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i]; - for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; - - for (i=0; i<3; i++) info->J2l[s2+i] = -p[i]; - for (i=0; i<3; i++) info->J2l[s3+i] = -q[i]; - } - for (i=0; i<3; i++) info->J1l[s2+i] = p[i]; - for (i=0; i<3; i++) info->J1l[s3+i] = q[i]; - // compute two elements of right hand side. we want to align the offset - // point (in body 2's frame) with the center of body 1. - btVector3 ofs; // offset point in global coordinates - if(m_body1) - { - ofs = trB.getOrigin() - trA.getOrigin(); - } - else - { - ofs = trA.getOrigin() - trB.getOrigin(); - } - k = info->fps * info->erp * slider->getSoftnessOrthoLin(); - info->c[2] = k * p.dot(ofs); - info->c[3] = k * q.dot(ofs); - int nrow = 3; // last filled row - int srow; - // check linear limits linear - btScalar limit_err = btScalar(0.0); - int limit = 0; - if(slider->getSolveLinLimit()) - { - limit_err = slider->getLinDepth(); - if(m_body1) - { - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - } - else - { - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - } - int powered = 0; - if(slider->getPoweredLinMotor()) - { - powered = 1; - } - // if the slider has joint limits, add in the extra row - if (limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->J1l[srow+0] = ax1[0]; - info->J1l[srow+1] = ax1[1]; - info->J1l[srow+2] = ax1[2]; - if(m_body1) - { - info->J2l[srow+0] = -ax1[0]; - info->J2l[srow+1] = -ax1[1]; - info->J2l[srow+2] = -ax1[2]; - } - // linear torque decoupling step: - // - // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in limited slider-jointed free - // bodies from gaining angular momentum. - // the solution used here is to apply the constraint forces at the point - // halfway between the body centers. there is no penalty (other than an - // extra tiny bit of computation) in doing this adjustment. note that we - // only need to do this if the constraint connects two bodies. - if (m_body1) - { - dVector3 ltd; // Linear Torque Decoupling vector (a torque) - c = btScalar(0.5) * c; - dCROSS (ltd,=,c,ax1); - info->J1a[srow+0] = ltd[0]; - info->J1a[srow+1] = ltd[1]; - info->J1a[srow+2] = ltd[2]; - info->J2a[srow+0] = ltd[0]; - info->J2a[srow+1] = ltd[1]; - info->J2a[srow+2] = ltd[2]; - } - // right-hand part - btScalar lostop = slider->getLowerLinLimit(); - btScalar histop = slider->getUpperLinLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - if(powered) - { - info->cfm[nrow] = btScalar(0.0); - if(!limit) - { - info->c[nrow] = slider->getTargetLinMotorVelocity(); - info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps; - info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps; - } - } - if(limit) - { - k = info->fps * info->erp; - if(m_body1) - { - info->c[nrow] = k * limit_err; - } - else - { - info->c[nrow] = - k * limit_err; - } - info->cfm[nrow] = btScalar(0.0); // stop_cfm; - if(lostop == histop) - { - // limited low and high simultaneously - info->lo[nrow] = -SIMD_INFINITY; - info->hi[nrow] = SIMD_INFINITY; - } - else - { - if(limit == 1) - { - // low limit - info->lo[nrow] = 0; - info->hi[nrow] = SIMD_INFINITY; - } - else - { - // high limit - info->lo[nrow] = -SIMD_INFINITY; - info->hi[nrow] = 0; - } - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) - btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimLin()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_body0->m_originalBody->getLinearVelocity().dot(ax1); - if(m_body1) - { - vel -= m_body1->m_originalBody->getLinearVelocity().dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { - // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if (newc > info->c[nrow]) info->c[nrow] = newc; - } - } - else - { - // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->c[nrow]) info->c[nrow] = newc; - } - } - } - info->c[nrow] *= slider->getSoftnessLimLin(); - } // if(limit) - } // if linear limit - // check angular limits - limit_err = btScalar(0.0); - limit = 0; - if(slider->getSolveAngLimit()) - { - limit_err = slider->getAngDepth(); - if(m_body1) - { - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - } - else - { - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - } - // if the slider has joint limits, add in the extra row - powered = 0; - if(slider->getPoweredAngMotor()) - { - powered = 1; - } - if(limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->J1a[srow+0] = ax1[0]; - info->J1a[srow+1] = ax1[1]; - info->J1a[srow+2] = ax1[2]; - if(m_body1) - { - info->J2a[srow+0] = -ax1[0]; - info->J2a[srow+1] = -ax1[1]; - info->J2a[srow+2] = -ax1[2]; - } - btScalar lostop = slider->getLowerAngLimit(); - btScalar histop = slider->getUpperAngLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - if(powered) - { - info->cfm[nrow] = btScalar(0.0); - if(!limit) - { - info->c[nrow] = slider->getTargetAngMotorVelocity(); - info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps; - info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps; - } - } - if(limit) - { - k = info->fps * info->erp; - if (m_body1) - { - info->c[nrow] = k * limit_err; - } - else - { - info->c[nrow] = -k * limit_err; - } - info->cfm[nrow] = btScalar(0.0); // stop_cfm; - if(lostop == histop) - { - // limited low and high simultaneously - info->lo[nrow] = -SIMD_INFINITY; - info->hi[nrow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - // low limit - info->lo[nrow] = 0; - info->hi[nrow] = SIMD_INFINITY; - } - else - { - // high limit - info->lo[nrow] = -SIMD_INFINITY; - info->hi[nrow] = 0; - } - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) - btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimAng()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_body0->m_originalBody->getAngularVelocity().dot(ax1); - if(m_body1) - { - vel -= m_body1->m_originalBody->getAngularVelocity().dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { - // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if (newc > info->c[nrow]) info->c[nrow] = newc; - } - } - else - { - // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->c[nrow]) info->c[nrow] = newc; - } - } - } - info->c[nrow] *= slider->getSoftnessLimAng(); - } // if(limit) - } // if angular limit or powered -} // OdeSliderJoint::GetInfo2() - -//---------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------- - - - - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h deleted file mode 100644 index a2affda382d..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h +++ /dev/null @@ -1,142 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 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. -*/ -/* -2007-09-09 -Added support for typed joints by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#ifndef TYPED_JOINT_H -#define TYPED_JOINT_H - -#include "btOdeJoint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" - -struct btOdeSolverBody; - -class btOdeTypedJoint : public btOdeJoint -{ -public: - btTypedConstraint * m_constraint; - int m_index; - bool m_swapBodies; - btOdeSolverBody* m_body0; - btOdeSolverBody* m_body1; - - btOdeTypedJoint(){} - btOdeTypedJoint( - btTypedConstraint * constraint, - int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1): - m_constraint(constraint), - m_index(index), - m_swapBodies(swap), - m_body0(body0), - m_body1(body1) - { - } - - virtual void GetInfo1(Info1 *info); - virtual void GetInfo2(Info2 *info); -}; - - - -class OdeP2PJoint : public btOdeTypedJoint -{ -protected: - inline btPoint2PointConstraint * getP2PConstraint() - { - return static_cast<btPoint2PointConstraint * >(m_constraint); - } -public: - - OdeP2PJoint() {}; - - OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1); - - //btOdeJoint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); -}; - - -class OdeD6Joint : public btOdeTypedJoint -{ -protected: - inline btGeneric6DofConstraint * getD6Constraint() - { - return static_cast<btGeneric6DofConstraint * >(m_constraint); - } - - int setLinearLimits(Info2 *info); - int setAngularLimits(Info2 *info, int row_offset); - -public: - - OdeD6Joint() {}; - - OdeD6Joint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1); - - //btOdeJoint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); -}; - -//! retrieves the constraint info from a btRotationalLimitMotor object -/*! \pre testLimitValue must be called on limot*/ -int bt_get_limit_motor_info2( - btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational); - -/* -OdeSliderJoint -Ported from ODE by Roman Ponomarev (rponom@gmail.com) -April 24, 2008 -*/ -class OdeSliderJoint : public btOdeTypedJoint -{ -protected: - inline btSliderConstraint * getSliderConstraint() - { - return static_cast<btSliderConstraint * >(m_constraint); - } -public: - - OdeSliderJoint() {}; - - OdeSliderJoint(btTypedConstraint* constraint,int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1); - - //BU_Joint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); -}; - - - - -#endif //CONTACT_JOINT_H - - - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index e7f07a428eb..4128f504bf1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -342,6 +342,7 @@ void btSliderConstraint::calculateTransforms(void){ void btSliderConstraint::testLinLimits(void) { m_solveLinLim = false; + m_linPos = m_depth[0]; if(m_lowerLinLimit <= m_upperLinLimit) { if(m_depth[0] > m_upperLinLimit) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index f69dfcf3aa7..580dfa1178d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -103,6 +103,8 @@ protected: btVector3 m_relPosA; btVector3 m_relPosB; + btScalar m_linPos; + btScalar m_angDepth; btScalar m_kAngle; @@ -191,6 +193,7 @@ public: btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } + btScalar getLinearPos() { return m_linPos; } // access for ODE solver bool getSolveLinLimit() { return m_solveLinLim; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp deleted file mode 100644 index 175d15dcfcf..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp +++ /dev/null @@ -1,683 +0,0 @@ -/* - * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - Bullet Continuous Collision Detection and Physics Library - Bullet is Copyright (c) 2003-2006 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 "btSorLcp.h" -#include "btOdeSolverBody.h" - -#ifdef USE_SOR_SOLVER - -// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver. -#include "LinearMath/btScalar.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include <math.h> -#include <float.h>//FLT_MAX -#ifdef WIN32 -#include <memory.h> -#endif -#include <string.h> -#include <stdio.h> - -#if defined (WIN32) -#include <malloc.h> -#else -#if defined (__FreeBSD__) -#include <stdlib.h> -#else -#include <alloca.h> -#endif -#endif - -#include "btOdeJoint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -//////////////////////////////////////////////////////////////////// -//math stuff -#include "btOdeMacros.h" - -//*************************************************************************** -// configuration - -// for the SOR and CG methods: -// uncomment the following line to use warm starting. this definitely -// help for motor-driven joints. unfortunately it appears to hurt -// with high-friction contacts using the SOR method. use with care - -//#define WARM_STARTING 1 - -// for the SOR method: -// uncomment the following line to randomly reorder constraint rows -// during the solution. depending on the situation, this can help a lot -// or hardly at all, but it doesn't seem to hurt. - -#define RANDOMLY_REORDER_CONSTRAINTS 1 - -//*************************************************************************** -// various common computations involving the matrix J -// compute iMJ = inv(M)*J' -inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, - //OdeSolverBody* const *body, - const btAlignedObjectArray<btOdeSolverBody*> &body, - dRealPtr invI) -{ - int i,j; - dRealMutablePtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; i<m; i++) { - int b1 = jb[i*2]; - int b2 = jb[i*2+1]; - btScalar k = body[b1]->m_invMass; - for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; - dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); - if (b2 >= 0) { - k = body[b2]->m_invMass; - for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; - dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); - } - J_ptr += 12; - iMJ_ptr += 12; - } -} - -#if 0 -static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) -{ - int i,j; - - - - dRealMutablePtr out_ptr1 = out + onlyBody1*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - - if (onlyBody2 >= 0) - { - out_ptr1 = out + onlyBody2*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - } - - dRealPtr iMJ_ptr = iMJ; - for (i=0; i<m; i++) { - - int b1 = jb[i*2]; - - dRealMutablePtr out_ptr = out + b1*6; - if ((b1 == onlyBody1) || (b1 == onlyBody2)) - { - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i] ; - } - - iMJ_ptr += 6; - - int b2 = jb[i*2+1]; - if ((b2 == onlyBody1) || (b2 == onlyBody2)) - { - if (b2 >= 0) - { - out_ptr = out + b2*6; - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i]; - } - } - - iMJ_ptr += 6; - - } -} -#endif - - -// compute out = inv(M)*J'*in. - -#if 0 -static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dSetZero1 (out,6*nb); - dRealPtr iMJ_ptr = iMJ; - for (i=0; i<m; i++) { - int b1 = jb[i*2]; - int b2 = jb[i*2+1]; - dRealMutablePtr out_ptr = out + b1*6; - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i]; - iMJ_ptr += 6; - if (b2 >= 0) { - out_ptr = out + b2*6; - for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; - } - iMJ_ptr += 6; - } -} -#endif - - -// compute out = J*in. -inline void multiply_J (int m, dRealMutablePtr J, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dRealPtr J_ptr = J; - for (i=0; i<m; i++) { - int b1 = jb[i*2]; - int b2 = jb[i*2+1]; - btScalar sum = 0; - dRealMutablePtr in_ptr = in + b1*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; - J_ptr += 6; - if (b2 >= 0) { - in_ptr = in + b2*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; - } - J_ptr += 6; - out[i] = sum; - } -} - -//*************************************************************************** -// SOR-LCP method - -// nb is the number of bodies in the body array. -// J is an m*12 matrix of constraint rows -// jb is an array of first and second body numbers for each constraint row -// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) -// -// this returns lambda and fc (the constraint force). -// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda -// -// b, lo and hi are modified on exit - -//------------------------------------------------------------------------------ -ATTRIBUTE_ALIGNED16(struct) IndexError { - btScalar error; // error to sort on - int findex; - int index; // row index -}; - -//------------------------------------------------------------------------------ -void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, - const btAlignedObjectArray<btOdeSolverBody*> &body, - dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, - dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, - int numiter,float overRelax, - btStackAlloc* stackAlloc - ) -{ - //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 - AutoBlockSa asaBlock(stackAlloc); - - const int num_iterations = numiter; - const float sor_w = overRelax; // SOR over-relaxation parameter - - int i,j; - -#ifdef WARM_STARTING - // for warm starting, this seems to be necessary to prevent - // jerkiness in motor-driven joints. i have no idea why this works. - for (i=0; i<m; i++) lambda[i] *= 0.9; -#else - dSetZero1 (lambda,m); -#endif - - // the lambda computed at the previous iteration. - // this is used to measure error for when we are reordering the indexes. - dRealAllocaArray (last_lambda,m); - - // a copy of the 'hi' vector in case findex[] is being used - dRealAllocaArray (hicopy,m); - memcpy (hicopy,hi,m*sizeof(float)); - - // precompute iMJ = inv(M)*J' - dRealAllocaArray (iMJ,m*12); - compute_invM_JT (m,J,iMJ,jb,body,invI); - - // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc - // as we change lambda. -#ifdef WARM_STARTING - multiply_invM_JT (m,nb,iMJ,jb,lambda,fc); -#else - dSetZero1 (invMforce,nb*6); -#endif - - // precompute 1 / diagonals of A - dRealAllocaArray (Ad,m); - dRealPtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; i<m; i++) { - float sum = 0; - for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j]; - if (jb[i*2+1] >= 0) { - for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; - } - iMJ_ptr += 12; - J_ptr += 12; - Ad[i] = sor_w / sum;//(sum + cfm[i]); - } - - // scale J and b by Ad - J_ptr = J; - for (i=0; i<m; i++) { - for (j=0; j<12; j++) { - J_ptr[0] *= Ad[i]; - J_ptr++; - } - rhs[i] *= Ad[i]; - } - - // scale Ad by CFM - for (i=0; i<m; i++) - Ad[i] *= cfm[i]; - - // order to solve constraint rows in - //IndexError *order = (IndexError*) alloca (m*sizeof(IndexError)); - IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError)); - - -#ifndef REORDER_CONSTRAINTS - // make sure constraints with findex < 0 come first. - j=0; - for (i=0; i<m; i++) - if (findex[i] < 0) - order[j++].index = i; - for (i=0; i<m; i++) - if (findex[i] >= 0) - order[j++].index = i; - dIASSERT (j==m); -#endif - - for (int iteration=0; iteration < num_iterations; iteration++) { - -#ifdef REORDER_CONSTRAINTS - // constraints with findex < 0 always come first. - if (iteration < 2) { - // for the first two iterations, solve the constraints in - // the given order - for (i=0; i<m; i++) { - order[i].error = i; - order[i].findex = findex[i]; - order[i].index = i; - } - } - else { - // sort the constraints so that the ones converging slowest - // get solved last. use the absolute (not relative) error. - for (i=0; i<m; i++) { - float v1 = dFabs (lambda[i]); - float v2 = dFabs (last_lambda[i]); - float max = (v1 > v2) ? v1 : v2; - if (max > 0) { - //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; - order[i].error = dFabs(lambda[i]-last_lambda[i]); - } - else { - order[i].error = dInfinity; - } - order[i].findex = findex[i]; - order[i].index = i; - } - } - qsort (order,m,sizeof(IndexError),&compare_index_error); -#endif -#ifdef RANDOMLY_REORDER_CONSTRAINTS - if ((iteration & 7) == 0) { - for (i=1; i<m; ++i) { - IndexError tmp = order[i]; - int swapi = dRandInt2(i+1); - order[i] = order[swapi]; - order[swapi] = tmp; - } - } -#endif - - //@@@ potential optimization: swap lambda and last_lambda pointers rather - // than copying the data. we must make sure lambda is properly - // returned to the caller - memcpy (last_lambda,lambda,m*sizeof(float)); - - for (int i=0; i<m; i++) { - // @@@ potential optimization: we could pre-sort J and iMJ, thereby - // linearizing access to those arrays. hmmm, this does not seem - // like a win, but we should think carefully about our memory - // access pattern. - - int index = order[i].index; - J_ptr = J + index*12; - iMJ_ptr = iMJ + index*12; - - // set the limits for this constraint. note that 'hicopy' is used. - // this is the place where the QuickStep method differs from the - // direct LCP solving method, since that method only performs this - // limit adjustment once per time step, whereas this method performs - // once per iteration per constraint row. - // the constraints are ordered so that all lambda[] values needed have - // already been computed. - if (findex[index] >= 0) { - hi[index] = btFabs (hicopy[index] * lambda[findex[index]]); - lo[index] = -hi[index]; - } - - int b1 = jb[index*2]; - int b2 = jb[index*2+1]; - float delta = rhs[index] - lambda[index]*Ad[index]; - dRealMutablePtr fc_ptr = invMforce + 6*b1; - - // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case - delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + - fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + - fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + - fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + - fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; - } - - // compute lambda and clamp it to [lo,hi]. - // @@@ potential optimization: does SSE have clamping instructions - // to save test+jump penalties here? - float new_lambda = lambda[index] + delta; - if (new_lambda < lo[index]) { - delta = lo[index]-lambda[index]; - lambda[index] = lo[index]; - } - else if (new_lambda > hi[index]) { - delta = hi[index]-lambda[index]; - lambda[index] = hi[index]; - } - else { - lambda[index] = new_lambda; - } - - //@@@ a trick that may or may not help - //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); - //delta *= ramp; - - // update invMforce. - // @@@ potential optimization: SIMD for this and the b2 >= 0 case - fc_ptr = invMforce + 6*b1; - fc_ptr[0] += delta * iMJ_ptr[0]; - fc_ptr[1] += delta * iMJ_ptr[1]; - fc_ptr[2] += delta * iMJ_ptr[2]; - fc_ptr[3] += delta * iMJ_ptr[3]; - fc_ptr[4] += delta * iMJ_ptr[4]; - fc_ptr[5] += delta * iMJ_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - fc_ptr[0] += delta * iMJ_ptr[6]; - fc_ptr[1] += delta * iMJ_ptr[7]; - fc_ptr[2] += delta * iMJ_ptr[8]; - fc_ptr[3] += delta * iMJ_ptr[9]; - fc_ptr[4] += delta * iMJ_ptr[10]; - fc_ptr[5] += delta * iMJ_ptr[11]; - } - } - } - //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 -} - -//------------------------------------------------------------------------------ -void btSorLcpSolver::SolveInternal1 ( - float global_cfm, - float global_erp, - const btAlignedObjectArray<btOdeSolverBody*> &body, int nb, - btAlignedObjectArray<btOdeJoint*> &joint, - int nj, const btContactSolverInfo& solverInfo, - btStackAlloc* stackAlloc) -{ - //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 - AutoBlockSa asaBlock(stackAlloc); - - int numIter = solverInfo.m_numIterations; - float sOr = solverInfo.m_sor; - - int i,j; - - btScalar stepsize1 = dRecip(solverInfo.m_timeStep); - - // number all bodies in the body list - set their tag values - for (i=0; i<nb; i++) - body[i]->m_odeTag = i; - - // make a local copy of the joint array, because we might want to modify it. - // (the "btOdeJoint *const*" declaration says we're allowed to modify the joints - // but not the joint array, because the caller might need it unchanged). - //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints - //btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*)); - //memcpy (joint,_joint,nj * sizeof(btOdeJoint*)); - - // for all bodies, compute the inertia tensor and its inverse in the global - // frame, and compute the rotational force and add it to the torque - // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. - dRealAllocaArray (I,3*4*nb); - dRealAllocaArray (invI,3*4*nb); -/* for (i=0; i<nb; i++) { - dMatrix3 tmp; - // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force - dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } -*/ - for (i=0; i<nb; i++) { - dMatrix3 tmp; - // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); - dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); - - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force -// dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity); -// dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp); - } - - - - - // get joint information (m = total constraint dimension, nub = number of unbounded variables). - // joints with m=0 are inactive and are removed from the joints array - // entirely, so that the code that follows does not consider them. - //@@@ do we really need to save all the info1's - btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1)); - - for (i=0, j=0; j<nj; j++) { // i=dest, j=src - joint[j]->GetInfo1 (info+i); - dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); - if (info[i].m > 0) { - joint[i] = joint[j]; - i++; - } - } - nj = i; - - // create the row offset array - int m = 0; - int *ofs = (int*) ALLOCA (nj*sizeof(int)); - for (i=0; i<nj; i++) { - ofs[i] = m; - m += info[i].m; - } - - // if there are constraints, compute the constraint force - dRealAllocaArray (J,m*12); - int *jb = (int*) ALLOCA (m*2*sizeof(int)); - if (m > 0) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. - dRealAllocaArray (c,m); - dRealAllocaArray (cfm,m); - dRealAllocaArray (lo,m); - dRealAllocaArray (hi,m); - - int *findex = (int*) ALLOCA (m*sizeof(int)); - - dSetZero1 (c,m); - dSetValue1 (cfm,m,global_cfm); - dSetValue1 (lo,m,-dInfinity); - dSetValue1 (hi,m, dInfinity); - for (i=0; i<m; i++) findex[i] = -1; - - // get jacobian data from constraints. an m*12 matrix will be created - // to store the two jacobian blocks from each constraint. it has this - // format: - // - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ . - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows) - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 / - // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows) - // etc... - // - // (lll) = linear jacobian data - // (aaa) = angular jacobian data - // - dSetZero1 (J,m*12); - btOdeJoint::Info2 Jinfo; - Jinfo.rowskip = 12; - Jinfo.fps = stepsize1; - Jinfo.erp = global_erp; - for (i=0; i<nj; i++) { - Jinfo.J1l = J + ofs[i]*12; - Jinfo.J1a = Jinfo.J1l + 3; - Jinfo.J2l = Jinfo.J1l + 6; - Jinfo.J2a = Jinfo.J1l + 9; - Jinfo.c = c + ofs[i]; - Jinfo.cfm = cfm + ofs[i]; - Jinfo.lo = lo + ofs[i]; - Jinfo.hi = hi + ofs[i]; - Jinfo.findex = findex + ofs[i]; - joint[i]->GetInfo2 (&Jinfo); - - if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) - Jinfo.c[0] = solverInfo.m_maxErrorReduction; - - // adjust returned findex values for global index numbering - for (j=0; j<info[i].m; j++) { - if (findex[ofs[i] + j] >= 0) - findex[ofs[i] + j] += ofs[i]; - } - } - - // create an array of body numbers for each joint row - int *jb_ptr = jb; - for (i=0; i<nj; i++) { - int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; - int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; - for (j=0; j<info[i].m; j++) { - jb_ptr[0] = b1; - jb_ptr[1] = b2; - jb_ptr += 2; - } - } - dIASSERT (jb_ptr == jb+2*m); - - // compute the right hand side `rhs' - dRealAllocaArray (tmp1,nb*6); - // put v/h + invM*fe into tmp1 - for (i=0; i<nb; i++) { - btScalar body_invMass = body[i]->m_invMass; - for (j=0; j<3; j++) - tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1; - dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); - for (j=0; j<3; j++) - tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1; - } - - // put J*tmp1 into rhs - dRealAllocaArray (rhs,m); - multiply_J (m,J,jb,tmp1,rhs); - - // complete rhs - for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i]; - - // scale CFM - for (i=0; i<m; i++) - cfm[i] *= stepsize1; - - // load lambda from the value saved on the previous iteration - dRealAllocaArray (lambda,m); -#ifdef WARM_STARTING - dSetZero1 (lambda,m); //@@@ shouldn't be necessary - for (i=0; i<nj; i++) { - memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar)); - } -#endif - - // solve the LCP problem and get lambda and invM*constraint_force - dRealAllocaArray (cforce,nb*6); - - /// SOR_LCP - SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc); - -#ifdef WARM_STARTING - // save lambda for the next iteration - //@@@ note that this doesn't work for contact joints yet, as they are - // recreated every iteration - for (i=0; i<nj; i++) { - memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar)); - } -#endif - - // note that the SOR method overwrites rhs and J at this point, so - // they should not be used again. - // add stepsize * cforce to the body velocity - for (i=0; i<nb; i++) { - for (j=0; j<3; j++) - body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j]; - for (j=0; j<3; j++) - body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; - - } - } - - // compute the velocity update: - // add stepsize * invM * fe to the body velocity - for (i=0; i<nb; i++) { - btScalar body_invMass = body[i]->m_invMass; - btVector3 linvel = body[i]->m_linearVelocity; - btVector3 angvel = body[i]->m_angularVelocity; - - for (j=0; j<3; j++) - { - linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; - } - for (j=0; j<3; j++) - { - body[i]->m_tacc[j] *= solverInfo.m_timeStep; - } - dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); - body[i]->m_angularVelocity = angvel; - } - //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 -} - - -#endif //USE_SOR_SOLVER diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h deleted file mode 100644 index 0d3583d33d9..00000000000 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - Bullet Continuous Collision Detection and Physics Library - Bullet is Copyright (c) 2003-2006 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. -*/ - -#define USE_SOR_SOLVER -#ifdef USE_SOR_SOLVER - -#ifndef SOR_LCP_H -#define SOR_LCP_H -struct btOdeSolverBody; -class btOdeJoint; -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btStackAlloc.h" - -struct btContactSolverInfo; - - -//============================================================================= -class btSorLcpSolver //Remotion: 11.10.2007 -{ -public: - btSorLcpSolver() - { - dRand2_seed = 0; - } - - void SolveInternal1 (float global_cfm, - float global_erp, - const btAlignedObjectArray<btOdeSolverBody*> &body, int nb, - btAlignedObjectArray<btOdeJoint*> &joint, - int nj, const btContactSolverInfo& solverInfo, - btStackAlloc* stackAlloc - ); - -public: //data - unsigned long dRand2_seed; - -protected: //typedef - typedef const btScalar *dRealPtr; - typedef btScalar *dRealMutablePtr; - -protected: //members - //------------------------------------------------------------------------------ - SIMD_FORCE_INLINE unsigned long dRand2() - { - dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff; - return dRand2_seed; - } - //------------------------------------------------------------------------------ - SIMD_FORCE_INLINE int dRandInt2 (int n) - { - float a = float(n) / 4294967296.0f; - return (int) (float(dRand2()) * a); - } - //------------------------------------------------------------------------------ - void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, - const btAlignedObjectArray<btOdeSolverBody*> &body, - dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, - dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, - int numiter,float overRelax, - btStackAlloc* stackAlloc - ); -}; - - -//============================================================================= -class AutoBlockSa //Remotion: 10.10.2007 -{ - btStackAlloc* stackAlloc; - btBlock* saBlock; -public: - AutoBlockSa(btStackAlloc* stackAlloc_) - { - stackAlloc = stackAlloc_; - saBlock = stackAlloc->beginBlock(); - } - ~AutoBlockSa() - { - stackAlloc->endBlock(saBlock); - } - //operator btBlock* () { return saBlock; } -}; -// //Usage -//void function(btStackAlloc* stackAlloc) -//{ -// AutoBlockSa(stackAlloc); -// ... -// if(...) return; -// return; -//} -//------------------------------------------------------------------------------ - - -#endif //SOR_LCP_H - -#endif //USE_SOR_SOLVER - diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index c7b1af245e9..e46c4e6136b 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -269,7 +269,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); body->getMotionState()->setWorldTransform(interpolatedTransform); } } @@ -708,7 +708,78 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() } +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback +{ + btCollisionObject* m_me; + btScalar m_allowedPenetration; + btOverlappingPairCache* m_pairCache; + + +public: + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : + btCollisionWorld::ClosestConvexResultCallback(fromA,toA), + m_allowedPenetration(0.0f), + m_me(me), + m_pairCache(pairCache) + { + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + { + if (convexResult.m_hitCollisionObject == m_me) + return 1.0; + + btVector3 linVelA,linVelB; + linVelA = m_convexToWorld-m_convexFromWorld; + linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); + + btVector3 relativeVelocity = (linVelA-linVelB); + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + return 1.f; + + return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + //don't collide with itself + if (proxy0->m_clientObject == m_me) + return false; + + ///don't do CCD when the collision filters are not matching + if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0)) + return false; + + ///don't do CCD when there are already contact points (touching contact/penetration) + btAlignedObjectArray<btPersistentManifold*> manifoldArray; + btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); + if (collisionPair) + { + if (collisionPair->m_algorithm) + { + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;j<manifoldArray.size();j++) + { + btPersistentManifold* manifold = manifoldArray[j]; + if (manifold->getNumContacts()>0) + return false; + } + } + } + return true; + } + + +}; + +///internal debugging variable. this value shouldn't be too high +int gNumClampedCcdMotions=0; + +//#include "stdio.h" void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); @@ -719,9 +790,34 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { + body->setHitFraction(1.f); + if (body->isActive() && (!body->isStaticOrKinematicObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + + if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + { + BT_PROFILE("CCD motion clamping"); + if (body->getCollisionShape()->isConvex()) + { + gNumClampedCcdMotions++; + + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); + btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) + { + body->setHitFraction(sweepResults.m_closestHitFraction); + body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->setHitFraction(0.f); +// printf("clamped integration to hit fraction = %f\n",fraction); + } + } + } + body->proceedToTransform( predictedTrans); } } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d206a604960..d9e2652aaf6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -59,7 +59,7 @@ protected: virtual void predictUnconstraintMotion(btScalar timeStep); - void integrateTransforms(btScalar timeStep); + virtual void integrateTransforms(btScalar timeStep); void calculateSimulationIslands(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index dd9dfa71b7f..929e24d337c 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -60,7 +60,7 @@ public: ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=10, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; virtual void debugDrawWorld() = 0; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index dfb48e2ff2d..2553009fec2 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -2002,7 +2002,7 @@ for(i=0;i<m_clusters.size();++i) mi.setMin(c.m_nodes[j]->m_x); mx.setMax(c.m_nodes[j]->m_x); } - const btDbvtVolume bounds=btDbvtVolume::FromMM(mi,mx); + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); if(c.m_leaf) m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); else @@ -2532,7 +2532,7 @@ switch(m_cfg.collisions&fCollision::RVSmask) const btScalar basemargin=getCollisionShape()->getMargin(); btVector3 mins; btVector3 maxs; - btDbvtVolume volume; + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), mins, maxs); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index e9ec81f4d2c..d9919967233 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -373,7 +373,6 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, bool areas, bool /*stress*/) { - /* for(int i=0;i<psb->m_nodes.size();++i) { const btSoftBody::Node& n=psb->m_nodes[i]; @@ -391,8 +390,6 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, } if(text[0]) idraw->draw3dText(n.m_x,text); } - */ - } // diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 0c87770d714..1a7be2cb840 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -750,7 +750,8 @@ struct btSoftColliders friction = btMin(psb->m_cfg.kDF,prb->getFriction()); btVector3 mins; btVector3 maxs; - btDbvtVolume volume; + + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); volume=btDbvtVolume::FromMM(mins,maxs); volume.Expand(btVector3(1,1,1)*margin); diff --git a/extern/bullet2/src/LinearMath/CMakeLists.txt b/extern/bullet2/src/LinearMath/CMakeLists.txt index 207eed94a3e..02ffaad7228 100644 --- a/extern/bullet2/src/LinearMath/CMakeLists.txt +++ b/extern/bullet2/src/LinearMath/CMakeLists.txt @@ -4,7 +4,32 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src } ) ADD_LIBRARY(LibLinearMath -btQuickprof.cpp -btGeometryUtil.cpp + btAlignedObjectArray.h + btList.h + btPoolAllocator.h + btRandom.h + btVector3.h + btDefaultMotionState.h + btMatrix3x3.h + btQuadWord.h + btHashMap.h + btScalar.h + btAabbUtil2.h + btConvexHull.h + btConvexHull.cpp + btMinMax.h + btQuaternion.h + btStackAlloc.h + btGeometryUtil.h + btMotionState.h + btTransform.h + btAlignedAllocator.h + btIDebugDraw.h + btPoint3.h + btQuickprof.h + btTransformUtil.h + btQuickprof.cpp + btGeometryUtil.cpp + btAlignedAllocator.cpp ) diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 8bb6b3af7c3..275c4914628 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -17,6 +17,7 @@ subject to the following restrictions: #ifndef AABB_UTIL2 #define AABB_UTIL2 +#include "btTransform.h" #include "btVector3.h" #include "btMinMax.h" @@ -163,6 +164,39 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, } + +SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut) +{ + btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin), + abs_b[1].dot(halfExtentsWithMargin), + abs_b[2].dot(halfExtentsWithMargin)); + aabbMinOut = center - extent; + aabbMaxOut = center + extent; +} + + +SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut) +{ + btAssert(localAabbMin.getX() <= localAabbMax.getX()); + btAssert(localAabbMin.getY() <= localAabbMax.getY()); + btAssert(localAabbMin.getZ() <= localAabbMax.getZ()); + btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + localHalfExtents+=btVector3(margin,margin,margin); + + btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); + btMatrix3x3 abs_b = trans.getBasis().absolute(); + btVector3 center = trans(localCenter); + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMinOut = center-extent; + aabbMaxOut = center+extent; +} + + #endif diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index ca1a801402f..14aa4ae2348 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -284,6 +284,91 @@ class btMatrix3x3 { } + ///diagonalizes this matrix by the Jacobi method. rot stores the rotation + ///from the coordinate system in which the matrix is diagonal to the original + ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration + ///stops when all off-diagonal elements are less than the threshold multiplied + ///by the sum of the absolute values of the diagonal, or when maxSteps have + ///been executed. Note that this matrix is assumed to be symmetric. + void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + { + rot.setIdentity(); + for (int step = maxSteps; step > 0; step--) + { + // find off-diagonal element [p][q] with largest magnitude + int p = 0; + int q = 1; + int r = 2; + btScalar max = btFabs(m_el[0][1]); + btScalar v = btFabs(m_el[0][2]); + if (v > max) + { + q = 2; + r = 1; + max = v; + } + v = btFabs(m_el[1][2]); + if (v > max) + { + p = 1; + q = 2; + r = 0; + max = v; + } + + btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); + if (max <= t) + { + if (max <= SIMD_EPSILON * t) + { + return; + } + step = 1; + } + + // compute Jacobi rotation J which leads to a zero for element [p][q] + btScalar mpq = m_el[p][q]; + btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); + btScalar theta2 = theta * theta; + btScalar cos; + btScalar sin; + if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) + { + t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) + : 1 / (theta - btSqrt(1 + theta2)); + cos = 1 / btSqrt(1 + t * t); + sin = cos * t; + } + else + { + // approximation for large theta-value, i.e., a nearly diagonal matrix + t = 1 / (theta * (2 + btScalar(0.5) / theta2)); + cos = 1 - btScalar(0.5) * t * t; + sin = cos * t; + } + + // apply rotation to matrix (this = J^T * this * J) + m_el[p][q] = m_el[q][p] = 0; + m_el[p][p] -= t * mpq; + m_el[q][q] += t * mpq; + btScalar mrp = m_el[r][p]; + btScalar mrq = m_el[r][q]; + m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; + m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; + + // apply rotation to rot (rot = rot * J) + for (int i = 0; i < 3; i++) + { + btVector3& row = rot[i]; + mrp = row[p]; + mrq = row[q]; + row[p] = cos * mrp - sin * mrq; + row[q] = cos * mrq + sin * mrp; + } + } + } + + protected: btScalar cofac(int r1, int c1, int r2, int c2) const diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 1fee626d0c0..3c96857d4eb 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -24,7 +24,7 @@ subject to the following restrictions: #include <cfloat> #include <float.h> -#define BT_BULLET_VERSION 271 +#define BT_BULLET_VERSION 272 inline int btGetVersion() { @@ -65,7 +65,11 @@ inline int btGetVersion() #endif //__MINGW32__ #include <assert.h> +#if defined(DEBUG) || defined (_DEBUG) #define btAssert assert +#else + #define btAssert(x) +#endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) @@ -116,7 +120,13 @@ inline int btGetVersion() #ifndef assert #include <assert.h> #endif + +#if defined(DEBUG) || defined (_DEBUG) #define btAssert assert +#else + #define btAssert(x) +#endif + //btFullAssert is optional, slows down a lot #define btFullAssert(x) #define btLikely(_c) _c @@ -145,11 +155,16 @@ typedef float btScalar; #endif + #define BT_DECLARE_ALIGNED_ALLOCATOR() \ - SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ + SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ + SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ + SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ + SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h index b21d8b4f66c..4b14f6d00f3 100644 --- a/extern/bullet2/src/btBulletCollisionCommon.h +++ b/extern/bullet2/src/btBulletCollisionCommon.h @@ -43,7 +43,9 @@ subject to the following restrictions: ///Narrowphase Collision Detector #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" + +//btSphereBoxCollisionAlgorithm is broken, use gjk for now +//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" ///Dispatching and generation of collision pairs (broadphase) diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h index ce9bb1d968b..b95972cd15d 100644 --- a/extern/bullet2/src/btBulletDynamicsCommon.h +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -32,9 +32,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -///Optional ODE quickstep constraint solver, redistributed under ZLib license -#include "BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btOdeTypedJoint.h" + ///Vehicle simulation, with wheel contact simulated by raycasts #include "BulletDynamics/Vehicle/btRaycastVehicle.h" |