From b07bb88a2dd82fb2eb00a32de6f6ac1aff1bed91 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 16 Nov 2006 06:14:12 +0000 Subject: [PATCH] Added new EPA penetration depth solver improved gjk/minkowski sampling pd method added original solver variant, use btSequentialImpulseConstraintSolver2 Added Pierre Terdimans PD testbed --- .../EPAPenDepthDemo/PenetrationTestBullet.cpp | 54 +- Extras/EPA/EpaPenetrationDepthSolver.h | 4 +- VERSION | 2 +- changes.txt | 10 + configure.ac | 2 +- .../NarrowPhaseCollision/btGjkEpa.cpp | 1320 ++++++++--------- .../btGjkEpaPenetrationDepthSolver.h | 6 +- .../btSequentialImpulseConstraintSolver.cpp | 135 +- .../btSequentialImpulseConstraintSolver.h | 33 + 9 files changed, 858 insertions(+), 708 deletions(-) diff --git a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp index 0647197a8..d9f125df0 100644 --- a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp +++ b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp @@ -31,9 +31,14 @@ #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" +//We can use the Bullet EPA or sampling penetration depth solver, but comparison might be useful +//#define COMPARE_WITH_SOLID35_AND_OTHER_EPA 1 +#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA #include "../Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h" #include "../Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h" #include "../Extras/EPA/EpaPenetrationDepthSolver.h" +#endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA #define USE_ORIGINAL 1 #ifndef USE_ORIGINAL @@ -277,26 +282,35 @@ static float gDepth; static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) { static btSimplexSolverInterface simplexSolver; - //static Solid3JohnsonSimplexSolver simplexSolver; +#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA +// static Solid3JohnsonSimplexSolver simplexSolver2; +#endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA simplexSolver.reset(); btConvexHullShape convexA((float*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3)); btConvexHullShape convexB((float*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3)); - static btMinkowskiPenetrationDepthSolver Solver0; - static Solid3EpaPenetrationDepth Solver1; - static EpaPenetrationDepthSolver Solver2; - + static btGjkEpaPenetrationDepthSolver Solver0; + static btMinkowskiPenetrationDepthSolver Solver1; + +#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA + static Solid3EpaPenetrationDepth Solver2; + static EpaPenetrationDepthSolver Solver3; +#endif btConvexPenetrationDepthSolver* Solver; if(gMethod==0) Solver = &Solver0; else if(gMethod==1) - Solver = &Solver1; + Solver = &Solver1; +#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA + else if(gMethod==2) + Solver = &Solver2; else - Solver = &Solver2; + Solver = &Solver3; +#endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA #ifdef USE_ORIGINAL @@ -321,13 +335,16 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) btVector3 normal; btScalar depth; + btGjkEpaSolver::sResults results; + btScalar radialMargin = 0.01f; + btGjkEpaSolver::Collide(&convexA,hull0.mTransform, &convexB,hull1.mTransform, - 0.01,0.01, - witnesses,normal,depth); - if (depth>0) + radialMargin, + results); + if (results.depth>0) { - output.addContactPoint(normal,witnesses[1],-depth); + output.addContactPoint(results.normal,results.witnesses[1],-results.depth); } #endif return true; @@ -471,7 +488,11 @@ static void KeyboardCallback(unsigned char key, int x, int y) case ' ': gMethod++; - if(gMethod==3) gMethod=0; +#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA + if(gMethod==4) gMethod=0; +#else + if(gMethod==2) gMethod=0; +#endif break; case '4': @@ -675,13 +696,16 @@ static void RenderCallback() switch ( gMethod) { case 0: - sprintf(buf,"Minkowski sampling Penetration depth solver\n" ); + sprintf(buf,"Bullet GjkEpa Penetration depth solver (zlib free\n" ); break; case 1: - sprintf(buf,"Solid35 EPA Penetration depth solver\n" ); + sprintf(buf,"Bullet Minkowski sampling Penetration depth solver\n" ); break; case 2: - sprintf(buf,"EPA Penetration depth solver (WorkInProgress, zlib free\n" ); + sprintf(buf,"Solid35 EPA Penetration depth solver\n" ); + break; + case 3: + sprintf(buf,"EPA Penetration depth solver (Experimental/WorkInProgress, zlib free\n" ); break; default: sprintf(buf,"Unknown Penetration Depth\n" ); diff --git a/Extras/EPA/EpaPenetrationDepthSolver.h b/Extras/EPA/EpaPenetrationDepthSolver.h index 6a92abd8b..d281c9761 100644 --- a/Extras/EPA/EpaPenetrationDepthSolver.h +++ b/Extras/EPA/EpaPenetrationDepthSolver.h @@ -14,8 +14,8 @@ subject to the following restrictions: 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 EPA_PENETRATION_DEPTH_H -#define EPA_PENETRATION_DEPTH_H +#ifndef EPA2_PENETRATION_DEPTH_H +#define EPA2_PENETRATION_DEPTH_H /** * EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to diff --git a/VERSION b/VERSION index b49971824..ebec1c99a 100644 --- a/VERSION +++ b/VERSION @@ -1,3 +1,3 @@ -Bullet Collision Detection and Physics Library version 2.22 +Bullet Collision Detection and Physics Library version 2.32 http://bullet.sourceforge.net diff --git a/changes.txt b/changes.txt index 341a17213..d325b2a7b 100644 --- a/changes.txt +++ b/changes.txt @@ -1,6 +1,16 @@ Bullet Continuous Collision Detection and Physics Library Erwin Coumans +2006 Nov 15 + Added EPA penetration depth algorithm, Expanding Polytope Algorithm + Added Pierre Terdiman penetration depth comparison/test DEMO + Fixed Bullet's Minkowski sampling penetration depth solver + Contributed by Nathanael Presson + +2006 Nov 11 + Added GIMPACT trimesh collision detection: concave versus concave, + Contributed by Francisco León Nájera + 2006 Nov 2 Minor refactoring: btCollisionObject changes from struct into class, added accessor methods Force use of btMotionState to synchronize graphics transform, disabled old btRigidBody constructor that accepts btTransform diff --git a/configure.ac b/configure.ac index 11ae971a6..ee0d1f1b4 100644 --- a/configure.ac +++ b/configure.ac @@ -9,7 +9,7 @@ AC_PREREQ([2.54]) #---------------------------------------------------------------------------- AC_INIT( [bullet], - [2.20], + [2.32], [bullet@erwincoumans.com]) CS_PACKAGEINFO( [Bullet Continuous Collision Detection and Physics Library], diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp index b34ec691b..2e4e9a8cb 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp @@ -16,663 +16,663 @@ subject to the following restrictions: /* GJK-EPA collision solver by Nathanael Presson Nov.2006 -*/ - - -#include "btGjkEpa.h" - - -namespace gjkepa_impl -{ - -// -// Port. typedefs -// - -typedef btScalar F; -typedef bool Z; -typedef int I; -typedef unsigned int U; -typedef unsigned char U1; -typedef unsigned short U2; - -typedef btVector3 Vector3; -typedef btMatrix3x3 Rotation; - -// -// Config -// - -#if 0 -#define BTLOCALSUPPORT localGetSupportingVertexWithoutMargin -#else -#define BTLOCALSUPPORT localGetSupportingVertex -#endif - -// -// Const -// - -static const U chkPrecision =1/U(sizeof(F)==4); - -static const F cstInf =F(1/sin(0.)); -static const F cstPi =F(acos(-1.)); - -static const U GJK_maxiterations =128; -static const U GJK_hashsize =1<<6; -static const U GJK_hashmask =GJK_hashsize-1; -static const F GJK_insimplex_eps =F(0.0001); - -static const U EPA_maxiterations =256; -static const F EPA_accuracy =F(1./1000./* of meters*/); - -// -// Utils -// - -static inline F Abs(F v) { return(v<0?-v:v); } -template static inline void Swap(T& a,T& b) { T -t(a);a=b;b=t; } -template static inline T Min(const T& a,const T& b) { -return(a static inline T Max(const T& a,const T& b) { -return(a>b?a:b); } -static inline void ClearMemory(void* p,U sz) { memset(p,0,(size_t)sz); -} -#if 0 -template static inline void Raise(const T& object) { -throw(object); } -#else -template static inline void Raise(const T&) {} -#endif - -// -// StackAlloc -// -struct StackAlloc - { - struct Block - { - Block* previous; - U1* address; - }; - StackAlloc() { ctor(); } - StackAlloc(U size) { ctor();Create(size); } - ~StackAlloc() { Free(); } - void Create(U size) - { - Free(); - data = new U1[size]; - totalsize = size; - } - StackAlloc* CreateChild(U size) - { - StackAlloc* sa(Allocate()); - sa->ischild = true; - sa->data = Allocate(size); - sa->totalsize = size; - sa->usedsize = 0; - sa->current = 0; - return(sa); - } - void Free() - { - if(usedsize==0) - { - if(!ischild) delete[] data; - data = 0; - usedsize = 0; - } else Raise(L"StackAlloc is still in use"); - } - Block* BeginBlock() - { - Block* pb(Allocate()); - pb->previous = current; - pb->address = data+usedsize; - current = pb; - return(pb); - } - void EndBlock(Block* block) - { - if(block==current) - { - current = block->previous; - usedsize = (U)((block->address-data)-sizeof(Block)); - } else Raise(L"Unmatched blocks"); - } - U1* Allocate(U size) - { - const U nus(usedsize+size); - if(nus T* Allocate() { -return((T*)Allocate((U)sizeof(T))); } - template T* AllocateArray(U count) { -return((T*)Allocate((U)sizeof(T)*count)); } - private: - void ctor() - { - data = 0; - totalsize = 0; - usedsize = 0; - current = 0; - ischild = false; - } - U1* data; - U totalsize; - U usedsize; - Block* current; - Z ischild; - }; - -// -// GJK -// -struct GJK - { - struct Mkv - { - Vector3 w; /* Minkowski vertice */ - Vector3 r; /* Ray */ - }; - struct He - { - Vector3 v; - He* n; - }; - static const U hashsize=64; - StackAlloc* sa; - StackAlloc::Block* sablock; - He* table[hashsize]; - Rotation wrotations[2]; - Vector3 positions[2]; - const btConvexShape* shapes[2]; - Mkv simplex[5]; - Vector3 ray; - U order; - F margin; - Z failed; - // - GJK(StackAlloc* psa, - const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0, - const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1, - F pmargin=0) - { - wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0; - wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1; - sa =psa; - sablock =sa->BeginBlock(); - margin =pmargin; - failed =false; - } - // - ~GJK() - { - sa->EndBlock(sablock); - } - // vdh : very dumm hash - static inline U Hash(const Vector3& v) - { - const U h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473)); - return(((*((const U*)&h))*169639)&GJK_hashmask); - } - // - inline Vector3 LocalSupport(const Vector3& d,U i) const - { - return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]); - } - // - inline void Support(const Vector3& d,Mkv& v) const - { - v.r = d; - v.w = LocalSupport(d,0)-LocalSupport(-d,1)+d*margin; - } - #define SPX(_i_) simplex[_i_] - #define SPXW(_i_) simplex[_i_].w - // - inline Z FetchSupport() - { - const U h(Hash(ray)); - He* e(table[h]); - while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } - e=sa->Allocate();e->v=ray;e->n=table[h];table[h]=e; - Support(ray,simplex[++order]); - return(ray.dot(SPXW(order))>0); - } - // - inline Z SolveSimplex2(const Vector3& ao,const Vector3& ab) - { - if(ab.dot(ao)>=0) - { - const Vector3 cabo(cross(ab,ao)); - if(cabo.length2()>GJK_insimplex_eps) - { ray=cross(cabo,ab); } - else - { return(true); } - } - else - { order=0;SPX(0)=SPX(1);ray=ao; } - return(false); - } - // - inline Z SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& -ac) - { - return(SolveSimplex3a(ao,ab,ac,cross(ab,ac))); - } - // - inline Z SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& -ac,const Vector3& cabc) - { - if((cross(cabc,ab)).dot(ao)<0) - { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } - else if((cross(cabc,ac)).dot(ao)>0) - { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); } - else - { - const F d(cabc.dot(ao)); - if(Abs(d)>GJK_insimplex_eps) - { - if(d>0) - { ray=cabc; } - else - { ray=-cabc;Swap(SPX(0),SPX(1)); } - return(false); - } else return(true); - } - } - // - inline Z SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& -ac,const Vector3& ad) - { - Vector3 crs; - if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps) - { -order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); -} - else if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps) - { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); } - else if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps) - { -order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); -} - else return(true); - } - // - inline Z SearchOrigin(const Vector3& initray=Vector3(1,0,0)) - { - static const U maxiterations(128); - U iterations(maxiterations); - order = 0; - failed = false; - Support(initray,simplex[0]);ray=-SPXW(0); - ClearMemory(table,sizeof(void*)*hashsize); - do { - const F rl(ray.length()); - ray/=rl>0?rl:1; - if(FetchSupport()) - { - Z found(false); - switch(order) - { - case 1: found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break; - case 2: found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break; - case 3: found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break; - } - if(found) return(true); - } else return(false); - } while(--iterations); - failed=true; - return(false); - } - // - inline Z EncloseOrigin() - { - switch(order) - { - /* Point */ - case 0: break; - /* Line TODO */ - case 1: break; - /* Triangle */ - case 2: - { - const -Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); - Support( n,simplex[++order]); - Support(-n,simplex[++order]); - return(true); - } - break; - /* Tetrahedron */ - case 3: return(true); - /* Hexahedron */ - case 4: return(true); - } - return(false); - } - #undef SPX - #undef SPXW - }; - -// -// EPA -// -struct EPA - { - // - struct Face - { - const GJK::Mkv* v[3]; - Face* f[3]; - U e[3]; - Vector3 n; - F d; - U mark; - Face* prev; - Face* next; - Face() {} - }; - // - EPA(GJK* pgjk) - { - gjk = pgjk; - sa = pgjk->sa; - } - // - ~EPA() - { - } - // - inline Vector3 GetCoordinates(const Face* face) const - { - const Vector3 o(face->n*-face->d); - const F a[]={ cross(face->v[0]->w-o,face->v[1]->w-o).length(), - cross(face->v[1]->w-o,face->v[2]->w-o).length(), - cross(face->v[2]->w-o,face->v[0]->w-o).length()}; - const F sm(a[0]+a[1]+a[2]); - return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1)); - } - // - inline Face* FindBest() const - { - Face* bf(0); - if(root) - { - Face* cf(root); - F bd(cstInf); - do { - if(cf->dd;bf=cf; } - } while(0!=(cf=cf->next)); - } - return(bf); - } - // - inline Z Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* -c) - { - const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); - const F len(nrm.length()); - f->v[0] = a; - f->v[1] = b; - f->v[2] = c; - f->mark = 0; - if(len>0) - { - f->n = nrm/len; - f->d = -f->n.dot(a->w); - return( (cross(a->w,b->w).dot(nrm)>=0)&& - (cross(b->w,c->w).dot(nrm)>=0)&& - (cross(c->w,a->w).dot(nrm)>=0)); - } - else - { - f->n = Vector3(1,0,0); - f->d = -cstInf; - invalid = true; - return(false); - } - } - // - inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) - { - Face* pf(sa->Allocate()); - if(Set(pf,a,b,c)) - { - if(root) root->prev=pf; - pf->prev=0; - pf->next=root; - root =pf; - ++nfaces; - } - else - { - pf->prev=pf->next=0; - } - return(pf); - } - // - inline void Detach(Face* face) - { - if(face->prev||face->next) - { - --nfaces; - if(face==root) - { root=face->next;root->prev=0; } - else - { - if(face->next==0) - { face->prev->next=0; } - else - { face->prev->next=face->next;face->next->prev=face->prev; } - } - face->prev=face->next=0; - } - } - // - inline void Link(Face* f0,U e0,Face* f1,U e1) - { - f0->f[e0]=f1;f1->e[e1]=e0; - f1->f[e1]=f0;f0->e[e0]=e1; - } - // - U BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& -ff) - { - static const U mod3[]={0,1,2,0,1}; - U ne(0); - if(f.mark!=markid) - { - const U e1(mod3[e+1]); - if((f.n.dot(w->w)+f.d)>0) - { - Face* nf(NewFace(f.v[e1],f.v[e],w)); - Link(nf,0,&f,e); - if(cf) Link(cf,1,nf,2); else ff=nf; - cf=nf;ne=1; - } - else - { - const U e2(mod3[e+2]); - Detach(&f); - f.mark = markid; - ne += BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff); - ne += BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff); - } - } - return(ne); - } - // - inline F EvaluatePD(F accuracy=0.0001) - { - static const U maxiterations(256); - StackAlloc::Block* sablock(sa->BeginBlock()); - U iterations(0); - Face* prevbestface(0); - Face* bestface(0); - U markid(1); - depth = -cstInf; - normal = Vector3(0,0,0); - root = 0; - nfaces = 0; - invalid = false; - /* Prepare hull */ - if(gjk->EncloseOrigin()) - { - GJK::Mkv* basemkv[5]; - Face* basefaces[6]; - U basecount(0); - switch(gjk->order) - { - case 3: - { - static const U fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; - static const -U eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; - for(U i=0;i<4;++i) { -basemkv[i]=sa->Allocate();*basemkv[i]=gjk->simplex[i]; } - for(U i=0;i<4;++i) { -basefaces[i]=NewFace(basemkv[fidx[i][0]],basemkv[fidx[i][1]],basemkv[fidx[i][2]]); -} - for(U i=0;i<6;++i) { -Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); } - basecount=4; - } - break; - case 4: - { - static const -U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; - static const -U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; - for(U i=0;i<5;++i) { -basemkv[i]=sa->Allocate();*basemkv[i]=gjk->simplex[i]; } - for(U i=0;i<6;++i) { -basefaces[i]=NewFace(basemkv[fidx[i][0]],basemkv[fidx[i][1]],basemkv[fidx[i][2]]); -} - for(U i=0;i<9;++i) { -Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); } - basecount=6; - } - break; - } - for(U i=0;id<0) { invalid=true;break; -} } - } - if(invalid||(0==nfaces)) - { - sa->EndBlock(sablock); - return(depth); - } - /* Expand hull */ - for(;iterationsAllocate()); - gjk->Support(-bf->n,*w); - prevbestface=bestface; - bestface =bf; - if((bf->n.dot(w->w)+bf->d)<-accuracy) - { - Face* cf(0); - Face* ff(0); - U nf(0); - Detach(bf); - bf->mark=++markid; - for(U i=0;i<3;++i) { -nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); } - if(invalid) { bestface=0;break; } - if(nf<=2) { break; } - Link(cf,1,ff,2); - } else break; - } else break; - } - /* Extract contact */ - if(bestface) - { - const Vector3 b(GetCoordinates(bestface)); - normal = bestface->n; - depth = Max(0,bestface->d); - for(U i=0;i<2;++i) - { - const F s(F(i?-1:1)); - for(U j=0;j<3;++j) - { - features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i); - } - } - nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z(); - nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z(); - } else failed=true; - sa->EndBlock(sablock); - return(depth); - } - // - GJK* gjk; - StackAlloc* sa; - Face* root; - U nfaces; - Vector3 features[2][3]; - Vector3 nearest[2]; - Vector3 normal; - F depth; - Z invalid; - Z failed; - }; - -} - -// -// Api -// - -using namespace gjkepa_impl; - -/* Need some kind of stackalloc , create a static one till bullet provide -one. */ -static const U g_sasize((1024<<10)*2); -static StackAlloc g_sa(g_sasize); - -// -bool btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0, - btConvexShape *shape1,const btTransform &wtrs1, - btScalar radialmargin, - sResults& results) -{ -/* Initialize */ -results.witnesses[0] = -results.witnesses[1] = -results.normal = Vector3(0,0,0); -results.depth = 0; -results.status = sResults::Separated; -/* Use GJK to locate origin */ -GJK gjk(&g_sa, - wtrs0.getBasis(),wtrs0.getOrigin(),shape0, - wtrs1.getBasis(),wtrs1.getOrigin(),shape1, - radialmargin); -if(gjk.SearchOrigin()) - { - /* Then EPA for penetration depth */ - EPA epa(&gjk); - const F pd(epa.EvaluatePD()); - if(pd>0) - { - results.status = sResults::Penetrating; - results.normal = epa.normal; - results.depth = pd; - results.witnesses[0] = epa.nearest[0]; - results.witnesses[1] = epa.nearest[1]; - return(true); - } else { if(epa.failed) results.status=sResults::EPA_Failed; } - } else { if(gjk.failed) results.status=sResults::GJK_Failed; } -return(false); -} - +*/ + + +#include "btGjkEpa.h" +#include //for memset + +namespace gjkepa_impl +{ + +// +// Port. typedefs +// + +typedef btScalar F; +typedef bool Z; +typedef int I; +typedef unsigned int U; +typedef unsigned char U1; +typedef unsigned short U2; + +typedef btVector3 Vector3; +typedef btMatrix3x3 Rotation; + +// +// Config +// + +#if 0 +#define BTLOCALSUPPORT localGetSupportingVertexWithoutMargin +#else +#define BTLOCALSUPPORT localGetSupportingVertex +#endif + +// +// Const +// + +static const U chkPrecision =1/U(sizeof(F)==4); + +static const F cstInf =F(1/sin(0.)); +static const F cstPi =F(acos(-1.)); + +static const U GJK_maxiterations =128; +static const U GJK_hashsize =1<<6; +static const U GJK_hashmask =GJK_hashsize-1; +static const F GJK_insimplex_eps =F(0.0001); + +static const U EPA_maxiterations =256; +static const F EPA_accuracy =F(1./1000./* of meters*/); + +// +// Utils +// + +static inline F Abs(F v) { return(v<0?-v:v); } +template static inline void Swap(T& a,T& b) { T +t(a);a=b;b=t; } +template static inline T Min(const T& a,const T& b) { +return(a static inline T Max(const T& a,const T& b) { +return(a>b?a:b); } +static inline void ClearMemory(void* p,U sz) { memset(p,0,(size_t)sz); +} +#if 0 +template static inline void Raise(const T& object) { +throw(object); } +#else +template static inline void Raise(const T&) {} +#endif + +// +// StackAlloc +// +struct StackAlloc + { + struct Block + { + Block* previous; + U1* address; + }; + StackAlloc() { ctor(); } + StackAlloc(U size) { ctor();Create(size); } + ~StackAlloc() { Free(); } + void Create(U size) + { + Free(); + data = new U1[size]; + totalsize = size; + } + StackAlloc* CreateChild(U size) + { + StackAlloc* sa(Allocate()); + sa->ischild = true; + sa->data = Allocate(size); + sa->totalsize = size; + sa->usedsize = 0; + sa->current = 0; + return(sa); + } + void Free() + { + if(usedsize==0) + { + if(!ischild) delete[] data; + data = 0; + usedsize = 0; + } else Raise(L"StackAlloc is still in use"); + } + Block* BeginBlock() + { + Block* pb(Allocate()); + pb->previous = current; + pb->address = data+usedsize; + current = pb; + return(pb); + } + void EndBlock(Block* block) + { + if(block==current) + { + current = block->previous; + usedsize = (U)((block->address-data)-sizeof(Block)); + } else Raise(L"Unmatched blocks"); + } + U1* Allocate(U size) + { + const U nus(usedsize+size); + if(nus T* Allocate() { +return((T*)Allocate((U)sizeof(T))); } + template T* AllocateArray(U count) { +return((T*)Allocate((U)sizeof(T)*count)); } + private: + void ctor() + { + data = 0; + totalsize = 0; + usedsize = 0; + current = 0; + ischild = false; + } + U1* data; + U totalsize; + U usedsize; + Block* current; + Z ischild; + }; + +// +// GJK +// +struct GJK + { + struct Mkv + { + Vector3 w; /* Minkowski vertice */ + Vector3 r; /* Ray */ + }; + struct He + { + Vector3 v; + He* n; + }; + static const U hashsize=64; + StackAlloc* sa; + StackAlloc::Block* sablock; + He* table[hashsize]; + Rotation wrotations[2]; + Vector3 positions[2]; + const btConvexShape* shapes[2]; + Mkv simplex[5]; + Vector3 ray; + U order; + F margin; + Z failed; + // + GJK(StackAlloc* psa, + const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0, + const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1, + F pmargin=0) + { + wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0; + wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1; + sa =psa; + sablock =sa->BeginBlock(); + margin =pmargin; + failed =false; + } + // + ~GJK() + { + sa->EndBlock(sablock); + } + // vdh : very dumm hash + static inline U Hash(const Vector3& v) + { + const U h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473)); + return(((*((const U*)&h))*169639)&GJK_hashmask); + } + // + inline Vector3 LocalSupport(const Vector3& d,U i) const + { + return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]); + } + // + inline void Support(const Vector3& d,Mkv& v) const + { + v.r = d; + v.w = LocalSupport(d,0)-LocalSupport(-d,1)+d*margin; + } + #define SPX(_i_) simplex[_i_] + #define SPXW(_i_) simplex[_i_].w + // + inline Z FetchSupport() + { + const U h(Hash(ray)); + He* e(table[h]); + while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } + e=sa->Allocate();e->v=ray;e->n=table[h];table[h]=e; + Support(ray,simplex[++order]); + return(ray.dot(SPXW(order))>0); + } + // + inline Z SolveSimplex2(const Vector3& ao,const Vector3& ab) + { + if(ab.dot(ao)>=0) + { + const Vector3 cabo(cross(ab,ao)); + if(cabo.length2()>GJK_insimplex_eps) + { ray=cross(cabo,ab); } + else + { return(true); } + } + else + { order=0;SPX(0)=SPX(1);ray=ao; } + return(false); + } + // + inline Z SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& +ac) + { + return(SolveSimplex3a(ao,ab,ac,cross(ab,ac))); + } + // + inline Z SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& +ac,const Vector3& cabc) + { + if((cross(cabc,ab)).dot(ao)<0) + { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } + else if((cross(cabc,ac)).dot(ao)>0) + { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); } + else + { + const F d(cabc.dot(ao)); + if(Abs(d)>GJK_insimplex_eps) + { + if(d>0) + { ray=cabc; } + else + { ray=-cabc;Swap(SPX(0),SPX(1)); } + return(false); + } else return(true); + } + } + // + inline Z SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& +ac,const Vector3& ad) + { + Vector3 crs; + if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps) + { +order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); +} + else if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps) + { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); } + else if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps) + { +order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); +} + else return(true); + } + // + inline Z SearchOrigin(const Vector3& initray=Vector3(1,0,0)) + { + static const U maxiterations(128); + U iterations(maxiterations); + order = 0; + failed = false; + Support(initray,simplex[0]);ray=-SPXW(0); + ClearMemory(table,sizeof(void*)*hashsize); + do { + const F rl(ray.length()); + ray/=rl>0?rl:1; + if(FetchSupport()) + { + Z found(false); + switch(order) + { + case 1: found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break; + case 2: found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break; + case 3: found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break; + } + if(found) return(true); + } else return(false); + } while(--iterations); + failed=true; + return(false); + } + // + inline Z EncloseOrigin() + { + switch(order) + { + /* Point */ + case 0: break; + /* Line TODO */ + case 1: break; + /* Triangle */ + case 2: + { + const +Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); + Support( n,simplex[++order]); + Support(-n,simplex[++order]); + return(true); + } + break; + /* Tetrahedron */ + case 3: return(true); + /* Hexahedron */ + case 4: return(true); + } + return(false); + } + #undef SPX + #undef SPXW + }; + +// +// EPA +// +struct EPA + { + // + struct Face + { + const GJK::Mkv* v[3]; + Face* f[3]; + U e[3]; + Vector3 n; + F d; + U mark; + Face* prev; + Face* next; + Face() {} + }; + // + EPA(GJK* pgjk) + { + gjk = pgjk; + sa = pgjk->sa; + } + // + ~EPA() + { + } + // + inline Vector3 GetCoordinates(const Face* face) const + { + const Vector3 o(face->n*-face->d); + const F a[]={ cross(face->v[0]->w-o,face->v[1]->w-o).length(), + cross(face->v[1]->w-o,face->v[2]->w-o).length(), + cross(face->v[2]->w-o,face->v[0]->w-o).length()}; + const F sm(a[0]+a[1]+a[2]); + return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1)); + } + // + inline Face* FindBest() const + { + Face* bf(0); + if(root) + { + Face* cf(root); + F bd(cstInf); + do { + if(cf->dd;bf=cf; } + } while(0!=(cf=cf->next)); + } + return(bf); + } + // + inline Z Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* +c) + { + const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); + const F len(nrm.length()); + f->v[0] = a; + f->v[1] = b; + f->v[2] = c; + f->mark = 0; + if(len>0) + { + f->n = nrm/len; + f->d = -f->n.dot(a->w); + return( (cross(a->w,b->w).dot(nrm)>=0)&& + (cross(b->w,c->w).dot(nrm)>=0)&& + (cross(c->w,a->w).dot(nrm)>=0)); + } + else + { + f->n = Vector3(1,0,0); + f->d = -cstInf; + invalid = true; + return(false); + } + } + // + inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) + { + Face* pf(sa->Allocate()); + if(Set(pf,a,b,c)) + { + if(root) root->prev=pf; + pf->prev=0; + pf->next=root; + root =pf; + ++nfaces; + } + else + { + pf->prev=pf->next=0; + } + return(pf); + } + // + inline void Detach(Face* face) + { + if(face->prev||face->next) + { + --nfaces; + if(face==root) + { root=face->next;root->prev=0; } + else + { + if(face->next==0) + { face->prev->next=0; } + else + { face->prev->next=face->next;face->next->prev=face->prev; } + } + face->prev=face->next=0; + } + } + // + inline void Link(Face* f0,U e0,Face* f1,U e1) + { + f0->f[e0]=f1;f1->e[e1]=e0; + f1->f[e1]=f0;f0->e[e0]=e1; + } + // + U BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& +ff) + { + static const U mod3[]={0,1,2,0,1}; + U ne(0); + if(f.mark!=markid) + { + const U e1(mod3[e+1]); + if((f.n.dot(w->w)+f.d)>0) + { + Face* nf(NewFace(f.v[e1],f.v[e],w)); + Link(nf,0,&f,e); + if(cf) Link(cf,1,nf,2); else ff=nf; + cf=nf;ne=1; + } + else + { + const U e2(mod3[e+2]); + Detach(&f); + f.mark = markid; + ne += BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff); + ne += BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff); + } + } + return(ne); + } + // + inline F EvaluatePD(F accuracy=0.0001) + { + static const U maxiterations(256); + StackAlloc::Block* sablock(sa->BeginBlock()); + U iterations(0); + Face* prevbestface(0); + Face* bestface(0); + U markid(1); + depth = -cstInf; + normal = Vector3(0,0,0); + root = 0; + nfaces = 0; + invalid = false; + /* Prepare hull */ + if(gjk->EncloseOrigin()) + { + GJK::Mkv* basemkv[5]; + Face* basefaces[6]; + U basecount(0); + switch(gjk->order) + { + case 3: + { + static const U fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; + static const +U eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; + for(U i=0;i<4;++i) { +basemkv[i]=sa->Allocate();*basemkv[i]=gjk->simplex[i]; } + for(U i=0;i<4;++i) { +basefaces[i]=NewFace(basemkv[fidx[i][0]],basemkv[fidx[i][1]],basemkv[fidx[i][2]]); +} + for(U i=0;i<6;++i) { +Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); } + basecount=4; + } + break; + case 4: + { + static const +U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; + static const +U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; + for(U i=0;i<5;++i) { +basemkv[i]=sa->Allocate();*basemkv[i]=gjk->simplex[i]; } + for(U i=0;i<6;++i) { +basefaces[i]=NewFace(basemkv[fidx[i][0]],basemkv[fidx[i][1]],basemkv[fidx[i][2]]); +} + for(U i=0;i<9;++i) { +Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); } + basecount=6; + } + break; + } + for(U i=0;id<0) { invalid=true;break; +} } + } + if(invalid||(0==nfaces)) + { + sa->EndBlock(sablock); + return(depth); + } + /* Expand hull */ + for(;iterationsAllocate()); + gjk->Support(-bf->n,*w); + prevbestface=bestface; + bestface =bf; + if((bf->n.dot(w->w)+bf->d)<-accuracy) + { + Face* cf(0); + Face* ff(0); + U nf(0); + Detach(bf); + bf->mark=++markid; + for(U i=0;i<3;++i) { +nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); } + if(invalid) { bestface=0;break; } + if(nf<=2) { break; } + Link(cf,1,ff,2); + } else break; + } else break; + } + /* Extract contact */ + if(bestface) + { + const Vector3 b(GetCoordinates(bestface)); + normal = bestface->n; + depth = Max(0,bestface->d); + for(U i=0;i<2;++i) + { + const F s(F(i?-1:1)); + for(U j=0;j<3;++j) + { + features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i); + } + } + nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z(); + nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z(); + } else failed=true; + sa->EndBlock(sablock); + return(depth); + } + // + GJK* gjk; + StackAlloc* sa; + Face* root; + U nfaces; + Vector3 features[2][3]; + Vector3 nearest[2]; + Vector3 normal; + F depth; + Z invalid; + Z failed; + }; + +} + +// +// Api +// + +using namespace gjkepa_impl; + +/* Need some kind of stackalloc , create a static one till bullet provide +one. */ +static const U g_sasize((1024<<10)*2); +static StackAlloc g_sa(g_sasize); + +// +bool btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0, + btConvexShape *shape1,const btTransform &wtrs1, + btScalar radialmargin, + sResults& results) +{ +/* Initialize */ +results.witnesses[0] = +results.witnesses[1] = +results.normal = Vector3(0,0,0); +results.depth = 0; +results.status = sResults::Separated; +/* Use GJK to locate origin */ +GJK gjk(&g_sa, + wtrs0.getBasis(),wtrs0.getOrigin(),shape0, + wtrs1.getBasis(),wtrs1.getOrigin(),shape1, + radialmargin); +if(gjk.SearchOrigin()) + { + /* Then EPA for penetration depth */ + EPA epa(&gjk); + const F pd(epa.EvaluatePD()); + if(pd>0) + { + results.status = sResults::Penetrating; + results.normal = epa.normal; + results.depth = pd; + results.witnesses[0] = epa.nearest[0]; + results.witnesses[1] = epa.nearest[1]; + return(true); + } else { if(epa.failed) results.status=sResults::EPA_Failed; } + } else { if(gjk.failed) results.status=sResults::GJK_Failed; } +return(false); +} + diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 6c7f008fa..b9615ec8a 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -14,8 +14,8 @@ subject to the following restrictions: 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 EPA_PENETRATION_DEPTH_H -#define EPA_PENETRATION_DEPTH_H +#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H +#define BT_GJP_EPA_PENETRATION_DEPTH_H #include "btConvexPenetrationDepthSolver.h" @@ -35,5 +35,5 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver }; -#endif // EPA_PENETRATION_DEPTH_H +#endif // BT_GJP_EPA_PENETRATION_DEPTH_H diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 568bcc409..cf33b1d67 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -63,8 +63,14 @@ bool MyContactDestroyedCallback(void* userPersistentData) return true; } +btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2() +{ + setSolverMode(SOLVER_USE_WARMSTARTING); +} + btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +:m_solverMode(SOLVER_RANDMIZE_ORDER) { gContactDestroyedCallback = &MyContactDestroyedCallback; @@ -92,18 +98,13 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man int totalPoints = 0; - { - int j; - for (j=0;jgetNumContacts();p++) { gOrder[totalPoints].m_manifoldIndex = j; @@ -113,36 +114,111 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man } } + //should traverse the contacts random order... + int iteration; + + { + for ( iteration = 0;iterationgetBody0(), + (btRigidBody*)manifold->getBody1() + ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + } + + for (j=0;jgetBody0(), + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + } + } + } + +#ifdef USE_PROFILE + btProfiler::endBlock("solve"); +#endif //USE_PROFILE + + return 0.f; +} + + +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + + btContactSolverInfo info = infoGlobal; + + int numiter = infoGlobal.m_numIterations; +#ifdef USE_PROFILE + btProfiler::beginBlock("solve"); +#endif //USE_PROFILE + + { + int j; + for (j=0;jgetNumContacts();p++) + { + //interleaving here gives better results + solve( (btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1() + ,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer); + } + } + } //should traverse the contacts random order... int iteration; + for ( iteration = 0;iterationgetNumContacts();p++) + { + solve( (btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1() + ,manifold->getContactPoint(p),info,iteration,debugDrawer); } } - - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + btPersistentManifold* manifold = manifoldPtr[j]; + for (int p=0;pgetNumContacts();p++) + { + solveFriction((btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer); + } } } + #ifdef USE_PROFILE btProfiler::endBlock("solve"); @@ -264,7 +340,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol float relaxation = info.m_damping; - cpd->m_appliedImpulse =0.f;//*= relaxation; + if (m_solverMode & SOLVER_USE_WARMSTARTING) + { + cpd->m_appliedImpulse *= relaxation; + } else + { + cpd->m_appliedImpulse =0.f; + } + //for friction cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 8264f5071..bcc3806b9 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -30,6 +30,7 @@ class btIDebugDraw; class btSequentialImpulseConstraintSolver : public btConstraintSolver { +protected: float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); @@ -37,8 +38,18 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + //choose between several modes, different friction model etc. + int m_solverMode; + public: + enum eSolverMode + { + SOLVER_RANDMIZE_ORDER = 1, + SOLVER_FRICTION_SEPARATE = 2, + SOLVER_USE_WARMSTARTING = 4 + }; + btSequentialImpulseConstraintSolver(); ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody @@ -59,7 +70,29 @@ public: virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + void setSolverMode(int mode) + { + m_solverMode = mode; + } + + int getSolverMode() const + { + return m_solverMode; + } }; +/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering +class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver +{ +public: + + btSequentialImpulseConstraintSolver2(); + + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + + +}; + + #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H