diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision')
23 files changed, 489 insertions, 861 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 2f41b3c2757..9ee83e7d561 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -96,7 +96,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD @@ -121,6 +121,10 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //not close enough while (dist > radius) { + if (result.m_debugDrawer) + { + result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); + } numIter++; if (numIter > maxIter) { @@ -170,6 +174,11 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); + if (result.m_debugDrawer) + { + result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); + } + result.DebugDraw( lambda ); btPointCollector pointCollector; @@ -197,6 +206,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //?? return false; } + } @@ -224,4 +234,3 @@ bool btContinuousConvexCollision::calcTimeOfImpact( */ } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 0edf4dcd496..b0bce341e41 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -41,7 +41,7 @@ public: virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} CastResult() - :m_fraction(btScalar(1e30)), + :m_fraction(btScalar(BT_LARGE_FLOAT)), m_debugDrawer(0), m_allowedPenetration(btScalar(0)) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 2989daeb44e..7e3fde8e291 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef CONVEX_PENETRATION_DEPTH_H -#define CONVEX_PENETRATION_DEPTH_H +#ifndef __CONVEX_PENETRATION_DEPTH_H +#define __CONVEX_PENETRATION_DEPTH_H class btStackAlloc; class btVector3; @@ -33,7 +33,7 @@ public: const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) = 0; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index b011bb9ae58..bc711ad495c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -33,21 +33,24 @@ struct btDiscreteCollisionDetectorInterface virtual ~Result(){} - ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0; + ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner + virtual void setShapeIdentifiersA(int partId0,int index0)=0; + virtual void setShapeIdentifiersB(int partId1,int index1)=0; virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(btScalar(1e30)) + :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)), + m_stackAlloc(0) { } btTransform m_transformA; btTransform m_transformB; btScalar m_maximumDistanceSquared; + btStackAlloc* m_stackAlloc; }; virtual ~btDiscreteCollisionDetectorInterface() {}; @@ -66,7 +69,7 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(btScalar(1e30)) + btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) { } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp deleted file mode 100644 index 36cdeeaefdb..00000000000 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ /dev/null @@ -1,628 +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. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson -Nov.2006 -*/ - -#include "btGjkEpa.h" -#include <string.h> //for memset -#include "LinearMath/btStackAlloc.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf -#ifdef __SPU__ -#include <spu_printf.h> -#define printf spu_printf -#endif //__SPU__ -#endif - -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 -// - - -#define cstInf SIMD_INFINITY -#define cstPi SIMD_PI -#define cst2Pi SIMD_2_PI -#define GJK_maxiterations (128) -#define GJK_hashsize (1<<6) -#define GJK_hashmask (GJK_hashsize-1) -#define GJK_insimplex_eps F(0.0001) -#define GJK_sqinsimplex_eps (GJK_insimplex_eps*GJK_insimplex_eps) -#define EPA_maxiterations 256 -#define EPA_inface_eps F(0.01) -#define EPA_accuracy F(0.001) - -// -// Utils -// - -static inline F Abs(F v) { return(v<0?-v:v); } -static inline F Sign(F v) { return(F(v<0?-1:1)); } -template <typename T> static inline void Swap(T& a,T& b) { T -t(a);a=b;b=t; } -template <typename T> static inline T Min(const T& a,const T& b) { -return(a<b?a:b); } -template <typename T> 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 <typename T> static inline void Raise(const T& object) { -throw(object); } -#else -template <typename T> static inline void Raise(const T&) {} -#endif - - - -// -// GJK -// -struct GJK - { - struct Mkv - { - Vector3 w; /* Minkowski vertice */ - Vector3 r; /* Ray */ - }; - struct He - { - Vector3 v; - He* n; - }; - btStackAlloc* sa; - btBlock* sablock; - He* table[GJK_hashsize]; - Rotation wrotations[2]; - Vector3 positions[2]; - const btConvexShape* shapes[2]; - Mkv simplex[5]; - Vector3 ray; - U order; - U iterations; - F margin; - Z failed; - // - GJK(btStackAlloc* 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) - { - //this doesn't compile under GCC 3.3.5, so add the ()... - //const U h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473)); - //return(((*((const U*)&h))*169639)&GJK_hashmask); - 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 = (He*)(table[h]); - while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } - e=(He*)sa->allocate(sizeof(He));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_sqinsimplex_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)<-GJK_insimplex_eps) - { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } - else if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps) - { 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)) - { - iterations = 0; - order = (U)-1; - failed = false; - ray = initray.normalized(); - ClearMemory(table,sizeof(void*)*GJK_hashsize); - FetchSupport(); - ray = -SPXW(0); - for(;iterations<GJK_maxiterations;++iterations) - { - 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); - } - failed=true; - return(false); - } - // - inline Z EncloseOrigin() - { - switch(order) - { - /* Point */ - case 0: break; - /* Line */ - case 1: - { - const Vector3 ab(SPXW(1)-SPXW(0)); - const Vector3 b[]={ cross(ab,Vector3(1,0,0)), - cross(ab,Vector3(0,1,0)), - cross(ab,Vector3(0,0,1))}; - const F m[]={b[0].length2(),b[1].length2(),b[2].length2()}; - const Rotation r(btQuaternion(ab.normalized(),cst2Pi/3)); - Vector3 w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]); - Support(w.normalized(),simplex[4]);w=r*w; - Support(w.normalized(),simplex[2]);w=r*w; - Support(w.normalized(),simplex[3]);w=r*w; - order=4; - return(true); - } - break; - /* Triangle */ - case 2: - { - const -Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); - Support( n,simplex[3]); - Support(-n,simplex[4]); - order=4; - 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() {} - }; - // - GJK* gjk; - btStackAlloc* sa; - Face* root; - U nfaces; - U iterations; - Vector3 features[2][3]; - Vector3 nearest[2]; - Vector3 normal; - F depth; - Z failed; - // - 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->d<bd) { bd=cf->d;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 - { - const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); - const F len(nrm.length()); - const Z valid( (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&& - (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&& - (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps)); - f->v[0] = a; - f->v[1] = b; - f->v[2] = c; - f->mark = 0; - f->n = nrm/(len>0?len:cstInf); - f->d = Max<F>(0,-f->n.dot(a->w)); - return(valid); - } - // - inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) - { - Face* pf = (Face*)sa->allocate(sizeof(Face)); - 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) const - { - f0->f[e0]=f1;f1->e[e1]=e0; - f1->f[e1]=f0;f0->e[e0]=e1; - } - // - GJK::Mkv* Support(const Vector3& w) const - { - GJK::Mkv* v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv)); - gjk->Support(w,*v); - return(v); - } - // - 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=EPA_accuracy) - { - btBlock* sablock = sa->beginBlock(); - Face* bestface = 0; - U markid(1); - depth = -cstInf; - normal = Vector3(0,0,0); - root = 0; - nfaces = 0; - iterations = 0; - failed = false; - /* Prepare hull */ - if(gjk->EncloseOrigin()) - { - const U* pfidx = 0; - U nfidx= 0; - const U* peidx = 0; - U neidx = 0; - GJK::Mkv* basemkv[5]; - Face* basefaces[6]; - switch(gjk->order) - { - /* Tetrahedron */ - 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}}; - pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6; - } break; - /* Hexahedron */ - 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}}; - pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9; - } break; - } - U i; - - for( i=0;i<=gjk->order;++i) { -basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; -} - for( i=0;i<nfidx;++i,pfidx+=3) { -basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); -} - for( i=0;i<neidx;++i,peidx+=4) { -Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); } - } - if(0==nfaces) - { - sa->endBlock(sablock); - return(depth); - } - /* Expand hull */ - for(;iterations<EPA_maxiterations;++iterations) - { - Face* bf = FindBest(); - if(bf) - { - GJK::Mkv* w = Support(-bf->n); - const F d(bf->n.dot(w->w)+bf->d); - bestface = bf; - if(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(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<F>(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); - } - }; -} - -// -// Api -// - -using namespace gjkepa_impl; - - - -// -bool btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0, - const btConvexShape *shape1,const btTransform &wtrs1, - btScalar radialmargin, - btStackAlloc* stackAlloc, - sResults& results) -{ - - -/* Initialize */ -results.witnesses[0] = -results.witnesses[1] = -results.normal = Vector3(0,0,0); -results.depth = 0; -results.status = sResults::Separated; -results.epa_iterations = 0; -results.gjk_iterations = 0; -/* Use GJK to locate origin */ -GJK gjk(stackAlloc, - wtrs0.getBasis(),wtrs0.getOrigin(),shape0, - wtrs1.getBasis(),wtrs1.getOrigin(),shape1, - radialmargin+EPA_accuracy); -const Z collide(gjk.SearchOrigin()); -results.gjk_iterations = static_cast<int>(gjk.iterations+1); -if(collide) - { - /* Then EPA for penetration depth */ - EPA epa(&gjk); - const F pd(epa.EvaluatePD()); - results.epa_iterations = static_cast<int>(epa.iterations+1); - 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/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h deleted file mode 100644 index 1c256f41939..00000000000 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h +++ /dev/null @@ -1,53 +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. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson -Nov.2006 -*/ - - -#ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -class btStackAlloc; - -///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver -{ -struct sResults - { - enum eStatus - { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - btScalar depth; - int epa_iterations; - int gjk_iterations; - }; -static bool Collide(const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btScalar radialmargin, - btStackAlloc* stackAlloc, - sResults& results); -}; - -#endif diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index ada20d3ef7a..f74261d4b21 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -68,7 +68,43 @@ namespace gjkepa2_impl const btConvexShape* m_shapes[2]; btMatrix3x3 m_toshape1; btTransform m_toshape0; +#ifdef __SPU__ + bool m_enableMargin; +#else btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif//__SPU__ + + + MinkowskiDiff() + { + + } +#ifdef __SPU__ + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) + { + return m_shapes[0]->localGetSupportVertexNonVirtual(d); + } else + { + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); + } + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) + { + return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); + } else + { + return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); + } + } +#else void EnableMargin(bool enable) { if(enable) @@ -84,6 +120,8 @@ namespace gjkepa2_impl { return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); } +#endif //__SPU__ + inline btVector3 Support(const btVector3& d) const { return(Support0(d)-Support1(-d)); @@ -202,7 +240,7 @@ namespace gjkepa2_impl lastw[clastw=(clastw+1)&3]=w; } /* Check for termination */ - const btScalar omega=dot(m_ray,w)/rl; + const btScalar omega=btDot(m_ray,w)/rl; alpha=btMax(omega,alpha); if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) {/* Return old simplex */ @@ -259,6 +297,9 @@ namespace gjkepa2_impl { case eStatus::Valid: m_distance=m_ray.length();break; case eStatus::Inside: m_distance=0;break; + default: + { + } } return(m_status); } @@ -288,7 +329,7 @@ namespace gjkepa2_impl { btVector3 axis=btVector3(0,0,0); axis[i]=1; - const btVector3 p=cross(d,axis); + const btVector3 p=btCross(d,axis); if(p.length2()>0) { appendvertice(*m_simplex, p); @@ -303,7 +344,7 @@ namespace gjkepa2_impl break; case 3: { - const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, m_simplex->c[2]->w-m_simplex->c[0]->w); if(n.length2()>0) { @@ -357,7 +398,7 @@ namespace gjkepa2_impl const btScalar l=d.length2(); if(l>GJK_SIMPLEX2_EPS) { - const btScalar t(l>0?-dot(a,d)/l:0); + const btScalar t(l>0?-btDot(a,d)/l:0); if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } @@ -372,16 +413,16 @@ namespace gjkepa2_impl static const U imd3[]={1,2,0}; const btVector3* vt[]={&a,&b,&c}; const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=cross(dl[0],dl[1]); + const btVector3 n=btCross(dl[0],dl[1]); const btScalar l=n.length2(); if(l>GJK_SIMPLEX3_EPS) { btScalar mindist=-1; - btScalar subw[2]; - U subm; + btScalar subw[2]={0.f,0.f}; + U subm(0); for(U i=0;i<3;++i) { - if(dot(*vt[i],cross(dl[i],n))>0) + if(btDot(*vt[i],btCross(dl[i],n))>0) { const U j=imd3[i]; const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); @@ -397,13 +438,13 @@ namespace gjkepa2_impl } if(mindist<0) { - const btScalar d=dot(a,n); + const btScalar d=btDot(a,n); const btScalar s=btSqrt(l); const btVector3 p=n*(d/l); mindist = p.length2(); m = 7; - w[0] = (cross(dl[1],b-p)).length()/s; - w[1] = (cross(dl[2],c-p)).length()/s; + w[0] = (btCross(dl[1],b-p)).length()/s; + w[1] = (btCross(dl[2],c-p)).length()/s; w[2] = 1-(w[0]+w[1]); } return(mindist); @@ -420,16 +461,16 @@ namespace gjkepa2_impl const btVector3* vt[]={&a,&b,&c,&d}; const btVector3 dl[]={a-d,b-d,c-d}; const btScalar vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; + const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) { btScalar mindist=-1; - btScalar subw[3]; - U subm; + btScalar subw[3]={0.f,0.f,0.f}; + U subm(0); for(U i=0;i<3;++i) { const U j=imd3[i]; - const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); if(s>0) { const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); @@ -601,7 +642,7 @@ namespace gjkepa2_impl bool valid=true; best->pass = (U1)(++pass); gjk.getsupport(best->n,*w); - const btScalar wdist=dot(best->n,w->w)-best->d; + const btScalar wdist=btDot(best->n,w->w)-best->d; if(wdist>EPA_ACCURACY) { for(U j=0;(j<3)&&valid;++j) @@ -628,11 +669,11 @@ namespace gjkepa2_impl m_result.c[0] = outer.c[0]; m_result.c[1] = outer.c[1]; m_result.c[2] = outer.c[2]; - m_result.p[0] = cross( outer.c[1]->w-projection, + m_result.p[0] = btCross( outer.c[1]->w-projection, outer.c[2]->w-projection).length(); - m_result.p[1] = cross( outer.c[2]->w-projection, + m_result.p[1] = btCross( outer.c[2]->w-projection, outer.c[0]->w-projection).length(); - m_result.p[2] = cross( outer.c[0]->w-projection, + m_result.p[2] = btCross( outer.c[0]->w-projection, outer.c[1]->w-projection).length(); const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; m_result.p[0] /= sum; @@ -666,18 +707,18 @@ namespace gjkepa2_impl face->c[0] = a; face->c[1] = b; face->c[2] = c; - face->n = cross(b->w-a->w,c->w-a->w); + face->n = btCross(b->w-a->w,c->w-a->w); const btScalar l=face->n.length(); const bool v=l>EPA_ACCURACY; face->p = btMin(btMin( - dot(a->w,cross(face->n,a->w-b->w)), - dot(b->w,cross(face->n,b->w-c->w))), - dot(c->w,cross(face->n,c->w-a->w))) / + btDot(a->w,btCross(face->n,a->w-b->w)), + btDot(b->w,btCross(face->n,b->w-c->w))), + btDot(c->w,btCross(face->n,c->w-a->w))) / (v?l:1); face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; if(v) { - face->d = dot(a->w,face->n)/l; + face->d = btDot(a->w,face->n)/l; face->n /= l; if(forced||(face->d>=-EPA_PLANE_EPS)) { @@ -715,7 +756,7 @@ namespace gjkepa2_impl if(f->pass!=pass) { const U e1=i1m3[e]; - if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) { sFace* nf=newface(f->c[e1],f->c[e],w,false); if(nf) @@ -854,10 +895,14 @@ bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, case GJK::eStatus::Failed: results.status=sResults::GJK_Failed; break; + default: + { + } } return(false); } +#ifndef __SPU__ // btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, btScalar margin, @@ -923,6 +968,7 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, else return(true); } +#endif //__SPU__ /* Symbols cleanup */ diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index a55214203d3..2296527d7db 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -55,7 +55,7 @@ static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, const btVector3& guess, sResults& results, bool usemargins=true); - +#ifndef __SPU__ static btScalar SignedDistance( const btVector3& position, btScalar margin, const btConvexShape* shape, @@ -66,6 +66,8 @@ static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs const btConvexShape* shape1,const btTransform& wtrs1, const btVector3& guess, sResults& results); +#endif //__SPU__ + }; #endif diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 55c23ee8549..c6dc3f3a672 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -25,17 +25,19 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw ) + class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { (void)debugDraw; (void)v; (void)simplexSolver; - const btScalar radialmargin(btScalar(0.)); +// const btScalar radialmargin(btScalar(0.)); btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); btGjkEpaSolver2::sResults results; + + if(btGjkEpaSolver2::Penetration(pConvexA,transformA, pConvexB,transformB, guessVector,results)) @@ -45,8 +47,18 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; + v = results.normal; return true; + } else + { + if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; } + } return false; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 4db18628021..a49689a1501 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -25,11 +25,15 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver { public : + btGjkEpaPenetrationDepthSolver() + { + } + bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw ); + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); private : diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 331d25623df..8af16b9cf6f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -38,20 +38,49 @@ int gNumDeepPenetrationChecks = 0; int gNumGjkChecks = 0; - btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), +m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_minkowskiA(objectA), +m_minkowskiB(objectB), +m_shapeTypeA(objectA->getShapeType()), +m_shapeTypeB(objectB->getShapeType()), +m_marginA(objectA->getMargin()), +m_marginB(objectB->getMargin()), +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(1) +{ +} +btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), +m_shapeTypeA(shapeTypeA), +m_shapeTypeB(shapeTypeB), +m_marginA(marginA), +m_marginB(marginB), m_ignoreMargin(false), m_lastUsedMethod(-1), m_catchDegeneracies(1) { } -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +{ + (void)swapResults; + + getClosestPointsNonVirtual(input,output,debugDraw); +} + +#ifdef __SPU__ +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#else +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#endif { m_cachedSeparatingDistance = 0.f; @@ -64,21 +93,10 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; -#ifdef __SPU__ - btScalar marginA = m_minkowskiA->getMarginNonVirtual(); - btScalar marginB = m_minkowskiB->getMarginNonVirtual(); -#else - btScalar marginA = m_minkowskiA->getMargin(); - btScalar marginB = m_minkowskiB->getMargin(); -#ifdef TEST_NON_VIRTUAL - btScalar marginAv = m_minkowskiA->getMarginNonVirtual(); - btScalar marginBv = m_minkowskiB->getMarginNonVirtual(); - btAssert(marginA == marginAv); - btAssert(marginB == marginBv); -#endif //TEST_NON_VIRTUAL -#endif - + bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); + btScalar marginA = m_marginA; + btScalar marginB = m_marginB; gNumGjkChecks++; @@ -107,7 +125,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_lastUsedMethod = -1; { - btScalar squaredDistance = SIMD_INFINITY; + btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); btScalar margin = marginA + marginB; @@ -123,6 +141,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); +#if 1 + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + +// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); +// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); + +#else #ifdef __SPU__ btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); @@ -136,6 +163,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btAssert((qInBv-qInB).length() < 0.0001); #endif // #endif //__SPU__ +#endif + btVector3 pWorld = localTransA(pInA); btVector3 qWorld = localTransB(qInB); @@ -144,12 +173,19 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& spu_printf("got local supporting vertices\n"); #endif + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + btVector3 w = pWorld - qWorld; delta = m_cachedSeparatingAxis.dot(w); // potential exit, they don't overlap if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) { + m_degenerateSimplex = 10; checkSimplex=true; //checkPenetration = false; break; @@ -171,6 +207,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; + } else + { + m_degenerateSimplex = 11; } checkSimplex = true; break; @@ -184,34 +223,52 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("addVertex 2\n"); #endif + btVector3 newCachedSeparatingAxis; + //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) + if (!m_simplexSolver->closest(newCachedSeparatingAxis)) { m_degenerateSimplex = 3; checkSimplex = true; break; } - if(m_cachedSeparatingAxis.length2()<REL_ERROR2) + if(newCachedSeparatingAxis.length2()<REL_ERROR2) { + m_cachedSeparatingAxis = newCachedSeparatingAxis; m_degenerateSimplex = 6; checkSimplex = true; break; } btScalar previousSquaredDistance = squaredDistance; - squaredDistance = m_cachedSeparatingAxis.length2(); + squaredDistance = newCachedSeparatingAxis.length2(); +#if 0 +///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo + if (squaredDistance>previousSquaredDistance) + { + m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } +#endif // + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); //are we getting any closer ? if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) { - m_simplexSolver->backup_closest(m_cachedSeparatingAxis); +// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); checkSimplex = true; + m_degenerateSimplex = 12; + break; } + m_cachedSeparatingAxis = newCachedSeparatingAxis; + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject if (m_curIter++ > gGjkMaxIter) { @@ -238,7 +295,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (!check) { //do we need this backup_closest here ? - m_simplexSolver->backup_closest(m_cachedSeparatingAxis); +// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; break; } } @@ -246,8 +304,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (checkSimplex) { m_simplexSolver->compute_points(pointOnA, pointOnB); - normalInB = pointOnA-pointOnB; - btScalar lenSqr = m_cachedSeparatingAxis.length2(); + normalInB = m_cachedSeparatingAxis; + btScalar lenSqr =m_cachedSeparatingAxis.length2(); + //valid normal if (lenSqr < 0.0001) { @@ -279,7 +338,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) { //penetration case - + //if there is no way to handle penetrations, bail out if (m_penetrationDepthSolver) { @@ -287,19 +346,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 tmpPointOnA,tmpPointOnB; gNumDeepPenetrationChecks++; + m_cachedSeparatingAxis.setZero(); bool isValid2 = m_penetrationDepthSolver->calcPenDepth( *m_simplexSolver, m_minkowskiA,m_minkowskiB, localTransA,localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw + debugDraw,input.m_stackAlloc ); + if (isValid2) { btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB = m_cachedSeparatingAxis; + lenSqr = m_cachedSeparatingAxis.length2(); + } + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); @@ -315,32 +382,62 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_lastUsedMethod = 3; } else { - + m_lastUsedMethod = 8; } } else { - //isValid = false; - m_lastUsedMethod = 4; + m_lastUsedMethod = 9; } } else + { - m_lastUsedMethod = 5; + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + + if (m_cachedSeparatingAxis.length2() > btScalar(0.)) + { + btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + pointOnA -= m_cachedSeparatingAxis * marginA ; + pointOnB += m_cachedSeparatingAxis * marginB ; + normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); + isValid = true; + m_lastUsedMethod = 6; + } else + { + m_lastUsedMethod = 5; + } + } } } + } } - if (isValid) - { -#ifdef __SPU__ - //spu_printf("distance\n"); -#endif //__CELLOS_LV2__ + + if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + { +#if 0 +///some debugging +// if (check2d) + { + printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]); + printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex); + } +#endif -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("output 1\n"); -#endif m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; @@ -349,10 +446,6 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& pointOnB+positionOffset, distance); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("output 2\n"); -#endif - //printf("gjk add:%f",distance); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 0ad4aab8a59..cc6287c86b0 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -36,6 +36,11 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface btSimplexSolverInterface* m_simplexSolver; const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; + int m_shapeTypeA; + int m_shapeTypeB; + btScalar m_marginA; + btScalar m_marginB; + bool m_ignoreMargin; btScalar m_cachedSeparatingDistance; @@ -50,10 +55,14 @@ public: btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + + void setMinkowskiA(btConvexShape* minkA) { m_minkowskiA = minkA; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index c7c9812985d..cd310570e06 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -19,7 +19,21 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransformUtil.h" - +#ifdef PFX_USE_FREE_VECTORMATH + #include "physics_effects\base_level\solver\pfx_constraint_row.h" +typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; +#else + // Don't change following order of parameters + ATTRIBUTE_ALIGNED16(struct) btConstraintRow { + btScalar m_normal[3]; + btScalar m_rhs; + btScalar m_jacDiagInv; + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_accumImpulse; + }; + typedef btConstraintRow PfxConstraintRow; +#endif //PFX_USE_FREE_VECTORMATH @@ -34,6 +48,10 @@ class btManifoldPoint m_lateralFrictionInitialized(false), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM1(0.f), + m_contactCFM2(0.f), m_lifeTime(0) { } @@ -52,10 +70,15 @@ class btManifoldPoint m_lateralFrictionInitialized(false), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM1(0.f), + m_contactCFM2(0.f), m_lifeTime(0) { - - + mConstraintRow[0].m_accumImpulse = 0.f; + mConstraintRow[1].m_accumImpulse = 0.f; + mConstraintRow[2].m_accumImpulse = 0.f; } @@ -83,11 +106,21 @@ class btManifoldPoint bool m_lateralFrictionInitialized; btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; + btScalar m_contactMotion1; + btScalar m_contactMotion2; + btScalar m_contactCFM1; + btScalar m_contactCFM2; + int m_lifeTime;//lifetime of the contactpoint in frames btVector3 m_lateralFrictionDir1; btVector3 m_lateralFrictionDir2; + + + btConstraintRow mConstraintRow[3]; + + btScalar getDistance() const { return m_distance1; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 1fdbb2457d1..fe31f08d61a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -20,63 +20,20 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #define NUM_UNITSPHERE_POINTS 42 -static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = -{ -btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), -btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), -btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), -btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), -btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), -btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), -btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), -btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), -btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), -btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), -btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), -btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), -btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), -btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), -btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), -btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), -btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), -btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), -btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), -btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), -btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), -btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), -btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), -btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), -btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), -btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), -btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), -btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), -btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), -btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), -btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), -btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), -btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), -btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), -btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), -btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), -btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), -btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), -btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), -btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), -btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), -btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) -}; bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) { + (void)stackAlloc; (void)v; + bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { @@ -90,10 +47,13 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btScalar m_depth; bool m_hasResult; - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + virtual void setShapeIdentifiersA(int partId0,int index0) { (void)partId0; (void)index0; + } + virtual void setShapeIdentifiersB(int partId1,int index1) + { (void)partId1; (void)index1; } @@ -107,7 +67,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s }; //just take fixed number of orientation, and sample the penetration depth in that direction - btScalar minProj = btScalar(1e30); + btScalar minProj = btScalar(BT_LARGE_FLOAT); btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); btVector3 minA,minB; btVector3 seperatingAxisInA,seperatingAxisInB; @@ -128,7 +88,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s for (i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; + btVector3 norm = getPenetrationDirections()[i]; seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; seperatingAxisInBBatch[i] = norm * transB.getBasis() ; } @@ -142,7 +102,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexA->getPreferredPenetrationDirection(i,norm); norm = transA.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; @@ -159,7 +119,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexB->getPreferredPenetrationDirection(i,norm); norm = transB.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; @@ -169,29 +129,44 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); for (i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; + btVector3 norm = getPenetrationDirections()[i]; + if (check2d) + { + norm[2] = 0.f; + } + if (norm.length2()>0.01) + { - pInA = supportVerticesABatch[i]; - qInB = supportVerticesBBatch[i]; + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; - pWorld = transA(pInA); - qWorld = transB(qInB); - w = qWorld - pWorld; - btScalar delta = norm.dot(w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = transA(pInA); + qWorld = transB(qInB); + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + w = qWorld - pWorld; + btScalar delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } } } #else @@ -208,7 +183,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexA->getPreferredPenetrationDirection(i,norm); norm = transA.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } @@ -223,7 +198,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 norm; convexB->getPreferredPenetrationDirection(i,norm); norm = transB.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; + getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } @@ -232,7 +207,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s for (int i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; + const btVector3& norm = getPenetrationDirections()[i]; seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInB = norm* transB.getBasis(); pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); @@ -260,7 +235,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s if (minProj < btScalar(0.)) return false; - minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); + btScalar extraSeparation = 0.5f;///scale dependent + minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); @@ -298,9 +274,10 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(1e30);//minProj; + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; btIntermediateResult res; + gjkdet.setCachedSeperatingAxis(-minNorm); gjkdet.getClosestPoints(input,res,debugDraw); btScalar correctedMinNorm = minProj - res.m_depth; @@ -309,12 +286,14 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //the penetration depth is over-estimated, relax it btScalar penetration_relaxation= btScalar(1.); minNorm*=penetration_relaxation; + if (res.m_hasResult) { pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; + v = minNorm; #ifdef DEBUG_DRAW if (debugDraw) @@ -329,5 +308,55 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s return res.m_hasResult; } +btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() +{ + static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = + { + btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), + btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), + btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), + btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), + btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), + btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), + btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), + btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), + btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), + btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), + btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), + btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), + btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), + btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), + btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), + btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), + btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), + btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), + btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), + btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), + btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), + btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), + btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), + btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), + btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), + btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), + btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), + btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), + btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), + btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) + }; + + return sPenetrationDirections; +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index e93e4e4bb4e..7b6c8a63779 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -22,13 +22,17 @@ subject to the following restrictions: ///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver { +protected: + + static btVector3* getPenetrationDirections(); + public: virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index eecf927ee10..924a8af87d1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -25,7 +25,8 @@ ContactProcessedCallback gContactProcessedCallback = 0; btPersistentManifold::btPersistentManifold() -:m_body0(0), +:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), +m_body0(0), m_body1(0), m_cachedPoints (0), m_index1a(0) @@ -172,9 +173,6 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) #if MANIFOLD_CACHE_SIZE >= 4 //sort cache so best points come first, based on area insertIndex = sortCachedPoints(newPoint); - - if (insertIndex<0) - insertIndex=0; #else insertIndex = 0; #endif @@ -183,7 +181,11 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) } else { m_cachedPoints++; + + } + if (insertIndex<0) + insertIndex=0; btAssert(m_pointCache[insertIndex].m_userPersistentData==0); m_pointCache[insertIndex] = newPoint; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 0b3c734d1d7..c8aac637307 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -30,9 +30,14 @@ extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); extern ContactDestroyedCallback gContactDestroyedCallback; +extern ContactProcessedCallback gContactProcessedCallback; - - +//the enum starts at 1024 to avoid type conflicts with btTypedConstraint +enum btContactManifoldTypes +{ + MIN_CONTACT_MANIFOLD_TYPE = 1024, + BT_PERSISTENT_MANIFOLD_TYPE +}; #define MANIFOLD_CACHE_SIZE 4 @@ -43,7 +48,10 @@ extern ContactDestroyedCallback gContactDestroyedCallback; ///reduces the cache to 4 points, when more then 4 points are added, using following rules: ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points ///note that some pairs of objects might have more then one contact manifold. -ATTRIBUTE_ALIGNED16( class) btPersistentManifold + + +ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject +//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -52,6 +60,7 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold /// void* will allow any rigidbody class void* m_body0; void* m_body1; + int m_cachedPoints; btScalar m_contactBreakingThreshold; @@ -67,16 +76,19 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); + int m_companionIdA; + int m_companionIdB; + int m_index1a; btPersistentManifold(); btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) - : m_body0(body0),m_body1(body1),m_cachedPoints(0), + : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), + m_body0(body0),m_body1(body1),m_cachedPoints(0), m_contactBreakingThreshold(contactBreakingThreshold), m_contactProcessingThreshold(contactProcessingThreshold) { - } SIMD_FORCE_INLINE void* getBody0() { return m_body0;} @@ -134,6 +146,10 @@ public: m_pointCache[index] = m_pointCache[lastUsedIndex]; //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; + m_pointCache[lastUsedIndex].mConstraintRow[0].m_accumImpulse = 0.f; + m_pointCache[lastUsedIndex].mConstraintRow[1].m_accumImpulse = 0.f; + m_pointCache[lastUsedIndex].mConstraintRow[2].m_accumImpulse = 0.f; + m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; @@ -151,10 +167,13 @@ public: #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; - + btScalar appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse; +// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; + + + btAssert(lifeTime>=0); void* cache = m_pointCache[insertIndex].m_userPersistentData; @@ -165,6 +184,11 @@ public: m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse = appliedImpulse; + m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse = appliedLateralImpulse1; + m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse = appliedLateralImpulse2; + + m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); @@ -175,7 +199,12 @@ public: bool validContactDistance(const btManifoldPoint& pt) const { - return pt.m_distance1 <= getContactBreakingThreshold(); + if (pt.m_lifeTime >1) + { + return pt.m_distance1 <= getContactBreakingThreshold(); + } + return pt.m_distance1 <= getContactProcessingThreshold(); + } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index 6262f44b9f1..6ca60548e71 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -31,17 +31,20 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result bool m_hasResult; btPointCollector () - : m_distance(btScalar(1e30)),m_hasResult(false) + : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false) { } - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + virtual void setShapeIdentifiersA(int partId0,int index0) { (void)partId0; (void)index0; + + } + virtual void setShapeIdentifiersB(int partId1,int index1) + { (void)partId1; (void)index1; - //?? } virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index cdb1d22444d..fbe579ce1e5 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -124,8 +124,9 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* m_convexShapeFrom = convexShapeFrom; m_convexShapeTo = convexShapeTo; m_triangleToWorld = triangleToWorld; - m_hitFraction = 1.0; - m_triangleCollisionMargin = triangleCollisionMargin; + m_hitFraction = 1.0f; + m_triangleCollisionMargin = triangleCollisionMargin; + m_allowedPenetration = 0.f; } void @@ -148,6 +149,7 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.); + castResult.m_allowedPenetration = m_allowedPenetration; if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) { //add hit diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 3a1ab388c13..bdd1add36d2 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -58,7 +58,8 @@ public: btTransform m_convexShapeTo; btTransform m_triangleToWorld; btScalar m_hitFraction; - btScalar m_triangleCollisionMargin; + btScalar m_triangleCollisionMargin; + btScalar m_allowedPenetration; btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index 4c709a8c3a9..18eb662de2f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -114,7 +114,10 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( hasResult = true; } } - m_simplexSolver->addVertex( w, supVertexA , supVertexB); + ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. + if (!m_simplexSolver->inSimplex(w)) + m_simplexSolver->addVertex( w, supVertexA , supVertexB); + if (m_simplexSolver->closest(v)) { dist2 = v.length2(); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index a7ffeda8c62..a775198ab29 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -68,7 +68,7 @@ void btVoronoiSimplexSolver::reset() m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); m_cachedBC.reset(); } @@ -289,7 +289,11 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) //w is in the current (reduced) simplex for (i=0;i<numverts;i++) { +#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD + if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) +#else if (m_simplexVectorW[i] == w) +#endif found = true; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index d3162d9fbeb..9a4f552924c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -24,6 +24,11 @@ subject to the following restrictions: #define VORONOI_SIMPLEX_MAX_VERTS 5 +///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure +#define BT_USE_EQUAL_VERTEX_THRESHOLD +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f + + struct btUsageBitfield{ btUsageBitfield() { @@ -106,8 +111,11 @@ public: btVector3 m_cachedP2; btVector3 m_cachedV; btVector3 m_lastW; + + btScalar m_equalVertexThreshold; bool m_cachedValidClosest; + btSubSimplexClosestResult m_cachedBC; bool m_needsUpdate; @@ -122,10 +130,23 @@ public: public: + btVoronoiSimplexSolver() + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + { + } void reset(); void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); + void setEqualVertexThreshold(btScalar threshold) + { + m_equalVertexThreshold = threshold; + } + + btScalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } bool closest(btVector3& v); |