kannonier8/blender
1
0
Fork 0
forked from bartvdbraak/blender
Code Pull Requests Activity

Upgrade to latest Bullet trunk, that is in sync with Blender/extern/bullet2. (except for one define 'WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER')

Browse Source 
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.
This commit is contained in:
Erwin Coumans 2008-09-13 07:06:43 +00:00
parent 206cfe7955
commit 7f293488d1
57 changed files with 1303 additions and 3471 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h vendored
View File

@ -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);

91
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp vendored
View File

@ -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");
}

198
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h vendored
View File

@ -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

66
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp vendored
View File

@ -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
}
}

21
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h vendored
View File

@ -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;

93
extern/bullet2/src/BulletCollision/CMakeLists.txt vendored
View File

@ -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
)

4
extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp vendored
View File

@ -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;

2
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp vendored
View File

@ -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)
{

18
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h vendored
View File

@ -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

13
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp vendored
View File

@ -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);
}
}
}

211
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp vendored
View File

@ -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);
}
}

7
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h vendored
View File

@ -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);
}
}

12
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp vendored
View File

@ -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))
{

3
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h vendored
View File

@ -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;

2
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp vendored
View File

@ -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);

14
extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp vendored
View File

@ -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);
}

3
extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h vendored
View File

@ -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;
}

190
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp vendored
View File

@ -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]);
}

42
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h vendored
View File

@ -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:

110
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp vendored
View File

@ -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(&centerCallback, -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;
}

7
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h vendored
View File

@ -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;
};

8
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp vendored
View File

@ -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;

2
extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp vendored
View File

@ -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,

24
extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h vendored
View File

@ -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);
}

103
extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp vendored Normal file
View File

@ -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);
}

57
extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h vendored Normal file
View File

@ -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

5
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp vendored
View File

@ -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()

10
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h vendored
View File

@ -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";}

20
extern/bullet2/src/BulletDynamics/CMakeLists.txt vendored
View File

@ -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
Dynamics/btDiscreteDynamicsWorld.cpp
Dynamics/btSimpleDynamicsWorld.cpp
ConstraintSolver/btTypedConstraint.h
Dynamics/Bullet-C-API.cpp
Dynamics/btDiscreteDynamicsWorld.cpp
Dynamics/btDiscreteDynamicsWorld.h
Dynamics/btSimpleDynamicsWorld.cpp
Dynamics/btSimpleDynamicsWorld.h
Dynamics/btRigidBody.cpp
Dynamics/btRigidBody.h
Vehicle/btRaycastVehicle.cpp
Vehicle/btRaycastVehicle.h
Vehicle/btWheelInfo.cpp
Vehicle/btWheelInfo.h
)

21
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp vendored
View File

@ -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);

9
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp vendored
View File

@ -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;

278
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp vendored
View File

@ -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
}

50
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h vendored
View File

@ -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

25
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp vendored
View File

@ -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()
{
}

94
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h vendored
View File

@ -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

212
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h vendored
View File

@ -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

393
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp vendored
View File

@ -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;
}
}

109
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h vendored
View File

@ -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

48
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h vendored
View File

@ -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

880
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp vendored
View File

@ -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()
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------

142
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h vendored
View File

@ -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

1
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp vendored
View File

@ -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)

3
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h vendored
View File

@ -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; }

683
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp vendored
View File

@ -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

112
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h vendored
View File

@ -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

98
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp vendored
View File

@ -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);
}
}

2
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h vendored
View File

@ -59,7 +59,7 @@ protected:
virtual void predictUnconstraintMotion(btScalar timeStep);
void integrateTransforms(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep);
void calculateSimulationIslands();

2
extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h vendored
View File

@ -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;

4
extern/bullet2/src/BulletSoftBody/btSoftBody.cpp vendored
View File

@ -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);

3
extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp vendored
View File

@ -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);
}
*/
}
//

3
extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h vendored
View File

@ -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);

29
extern/bullet2/src/LinearMath/CMakeLists.txt vendored
View File

@ -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
)

34
extern/bullet2/src/LinearMath/btAabbUtil2.h vendored
View File

@ -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

85
extern/bullet2/src/LinearMath/btMatrix3x3.h vendored
View File

@ -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

25
extern/bullet2/src/LinearMath/btScalar.h vendored
View File

@ -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*) { } \

4
extern/bullet2/src/btBulletCollisionCommon.h vendored
View File

@ -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)

4
extern/bullet2/src/btBulletDynamicsCommon.h vendored
View File

@ -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"