forked from bartvdbraak/blender
Upgrade to latest Bullet trunk, that is in sync with Blender/extern/bullet2. (except for one define 'WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER')
In case someone reads those SVN logs: you can enable some extra broadphase SSE optimizations by replacing WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER by WIN32 in extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h Thanks to Benoit Bolsee for the upstream patch/contribution. Removed some obsolete files, they were just intended for comparison/testing.
This commit is contained in:
parent
206cfe7955
commit
7f293488d1
@ -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,9 +272,8 @@ 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);
|
||||
|
@ -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");
|
||||
}
|
||||
|
@ -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)
|
||||
{
|
||||
@ -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
|
||||
|
@ -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++)
|
||||
{
|
||||
btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
|
||||
btAssert(proxy0 != proxy1);
|
||||
|
||||
if (proxy0 != proxy1)
|
||||
btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
|
||||
btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
|
||||
|
||||
if (aabbOverlap(p0,p1))
|
||||
{
|
||||
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->findPair(proxy0,proxy1))
|
||||
{
|
||||
m_pairCache->addOverlappingPair(proxy0,proxy1);
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (!m_pairCache->hasDeferredRemoval())
|
||||
{
|
||||
if ( m_pairCache->findPair(proxy0,proxy1))
|
||||
@ -211,14 +207,8 @@ 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())
|
||||
@ -273,8 +263,8 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
|
||||
{
|
||||
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++;
|
||||
@ -283,16 +273,16 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
|
||||
|
||||
}
|
||||
|
||||
///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
|
||||
|
||||
}
|
||||
}
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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
|
||||
)
|
||||
|
@ -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;
|
||||
|
@ -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)
|
||||
{
|
||||
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -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 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
const btTransform& childTrans = compoundShape->getChildTransform(i);
|
||||
btTransform newChildWorldTrans = orgTrans*childTrans ;
|
||||
colObj->setWorldTransform( newChildWorldTrans);
|
||||
colObj->setInterpolationWorldTransform(newChildWorldTrans);
|
||||
|
||||
//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);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -28,12 +28,14 @@ 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:
|
||||
|
||||
btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
|
||||
@ -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
12
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
vendored
@ -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))
|
||||
{
|
||||
|
@ -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
2
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
vendored
@ -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);
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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,58 +66,76 @@ 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.
|
||||
//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();
|
||||
}
|
||||
}
|
||||
|
||||
// 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();
|
||||
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));
|
||||
// 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];
|
||||
}
|
||||
}
|
||||
//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();
|
||||
@ -119,12 +143,11 @@ void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVect
|
||||
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
|
||||
@ -149,3 +172,60 @@ 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]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
@ -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,10 +68,10 @@ 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
|
||||
@ -104,8 +106,8 @@ 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)
|
||||
@ -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:
|
||||
|
@ -204,3 +204,113 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
|
||||
{
|
||||
return m_stridingMesh->getScaling();
|
||||
}
|
||||
|
||||
void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
|
||||
{
|
||||
class CenterCallback: public btInternalTriangleIndexCallback
|
||||
{
|
||||
bool first;
|
||||
btVector3 ref;
|
||||
btVector3 sum;
|
||||
btScalar volume;
|
||||
|
||||
public:
|
||||
|
||||
CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
|
||||
{
|
||||
(void) triangleIndex;
|
||||
(void) partId;
|
||||
if (first)
|
||||
{
|
||||
ref = triangle[0];
|
||||
first = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
|
||||
sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
|
||||
volume += vol;
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 getCenter()
|
||||
{
|
||||
return (volume > 0) ? sum / volume : ref;
|
||||
}
|
||||
|
||||
btScalar getVolume()
|
||||
{
|
||||
return volume * btScalar(1. / 6);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class InertiaCallback: public btInternalTriangleIndexCallback
|
||||
{
|
||||
btMatrix3x3 sum;
|
||||
btVector3 center;
|
||||
|
||||
public:
|
||||
|
||||
InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
|
||||
{
|
||||
(void) triangleIndex;
|
||||
(void) partId;
|
||||
btMatrix3x3 i;
|
||||
btVector3 a = triangle[0] - center;
|
||||
btVector3 b = triangle[1] - center;
|
||||
btVector3 c = triangle[2] - center;
|
||||
btVector3 abc = a + b + c;
|
||||
btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
for (int k = 0; k <= j; k++)
|
||||
{
|
||||
i[j][k] = i[k][j] = volNeg * (center[j] * center[k]
|
||||
+ btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j])
|
||||
+ btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
|
||||
+ btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
|
||||
}
|
||||
}
|
||||
btScalar i00 = -i[0][0];
|
||||
btScalar i11 = -i[1][1];
|
||||
btScalar i22 = -i[2][2];
|
||||
i[0][0] = i11 + i22;
|
||||
i[1][1] = i22 + i00;
|
||||
i[2][2] = i00 + i11;
|
||||
sum[0] += i[0];
|
||||
sum[1] += i[1];
|
||||
sum[2] += i[2];
|
||||
}
|
||||
|
||||
btMatrix3x3& getInertia()
|
||||
{
|
||||
return sum;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
CenterCallback centerCallback;
|
||||
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
|
||||
m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax);
|
||||
btVector3 center = centerCallback.getCenter();
|
||||
principal.setOrigin(center);
|
||||
volume = centerCallback.getVolume();
|
||||
|
||||
InertiaCallback inertiaCallback(center);
|
||||
m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
|
||||
|
||||
btMatrix3x3& i = inertiaCallback.getInertia();
|
||||
i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
|
||||
inertia.setValue(i[0][0], i[1][1], i[2][2]);
|
||||
inertia /= volume;
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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,
|
||||
|
@ -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
103
extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
vendored
Normal 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
57
extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
vendored
Normal 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
|
@ -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,8 +57,6 @@ 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;
|
||||
|
||||
|
@ -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
20
extern/bullet2/src/BulletDynamics/CMakeLists.txt
vendored
@ -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
|
||||
)
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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()
|
||||
{
|
||||
|
||||
}
|
@ -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
|
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
@ -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
|
@ -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
|
||||
|
@ -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()
|
||||
|
||||
//----------------------------------------------------------------------------------
|
||||
//----------------------------------------------------------------------------------
|
||||
|
||||
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
||||
|
@ -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)
|
||||
|
@ -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; }
|
||||
|
@ -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
|
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -59,7 +59,7 @@ protected:
|
||||
|
||||
virtual void predictUnconstraintMotion(btScalar timeStep);
|
||||
|
||||
void integrateTransforms(btScalar timeStep);
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
|
||||
void calculateSimulationIslands();
|
||||
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
//
|
||||
|
@ -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
29
extern/bullet2/src/LinearMath/CMakeLists.txt
vendored
@ -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
34
extern/bullet2/src/LinearMath/btAabbUtil2.h
vendored
@ -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
85
extern/bullet2/src/LinearMath/btMatrix3x3.h
vendored
@ -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
25
extern/bullet2/src/LinearMath/btScalar.h
vendored
@ -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
4
extern/bullet2/src/btBulletCollisionCommon.h
vendored
@ -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
4
extern/bullet2/src/btBulletDynamicsCommon.h
vendored
@ -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"
|
||||
|
Loading…
Reference in New Issue
Block a user