Back to index

supertuxkart  0.5+dfsg1
btGjkEpa.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the
00007 use of this software.
00008 Permission is granted to anyone to use this software for any purpose,
00009 including commercial applications, and to alter it and redistribute it
00010 freely,
00011 subject to the following restrictions:
00012 
00013 1. The origin of this software must not be misrepresented; you must not
00014 claim that you wrote the original software. If you use this software in a
00015 product, an acknowledgment in the product documentation would be appreciated
00016 but is not required.
00017 2. Altered source versions must be plainly marked as such, and must not be
00018 misrepresented as being the original software.
00019 3. This notice may not be removed or altered from any source distribution.
00020 */
00021 
00022 /*
00023 GJK-EPA collision solver by Nathanael Presson
00024 Nov.2006
00025 */
00026 
00027 #include "btGjkEpa.h"
00028 #include <string.h> //for memset
00029 #include "LinearMath/btStackAlloc.h"
00030 
00031 #if defined(DEBUG) || defined (_DEBUG)
00032 #include <stdio.h> //for debug printf
00033 #ifdef __SPU__
00034 #include <spu_printf.h>
00035 #define printf spu_printf
00036 #endif //__SPU__
00037 #endif
00038 
00039 namespace gjkepa_impl
00040 {
00041 
00042 //
00043 // Port. typedefs
00044 //
00045 
00046 typedef       btScalar             F;
00047 typedef bool                Z;
00048 typedef int                        I;
00049 typedef unsigned int U;
00050 typedef unsigned char       U1;
00051 typedef unsigned short      U2;
00052 
00053 typedef btVector3           Vector3;
00054 typedef btMatrix3x3         Rotation;
00055 
00056 //
00057 // Config
00058 //
00059 
00060 #if 0
00061 #define BTLOCALSUPPORT      localGetSupportingVertexWithoutMargin
00062 #else
00063 #define BTLOCALSUPPORT      localGetSupportingVertex
00064 #endif
00065 
00066 //
00067 // Const
00068 //
00069 
00070 
00071 #define cstInf                            SIMD_INFINITY
00072 #define cstPi                      SIMD_PI
00073 #define       cst2Pi                      SIMD_2_PI
00074 #define GJK_maxiterations   (128)
00075 #define GJK_hashsize        (1<<6)
00076 #define GJK_hashmask        (GJK_hashsize-1)
00077 #define GJK_insimplex_eps   F(0.0001)
00078 #define GJK_sqinsimplex_eps (GJK_insimplex_eps*GJK_insimplex_eps)
00079 #define EPA_maxiterations   256
00080 #define       EPA_inface_eps              F(0.01)
00081 #define EPA_accuracy        F(0.001)
00082 
00083 //
00084 // Utils
00085 //
00086 
00087 static inline F                                                       Abs(F v)                                  { return(v<0?-v:v); }
00088 static inline F                                                       Sign(F v)                                 { return(F(v<0?-1:1)); }
00089 template <typename T> static inline void  Swap(T& a,T& b)                           { T 
00090 t(a);a=b;b=t; }
00091 template <typename T> static inline T            Min(const T& a,const T& b)  { 
00092 return(a<b?a:b); }
00093 template <typename T> static inline T            Max(const T& a,const T& b)  { 
00094 return(a>b?a:b); }
00095 static inline void                                             ClearMemory(void* p,U sz)   { memset(p,0,(size_t)sz); 
00096 }
00097 #if 0
00098 template <typename T> static inline void  Raise(const T& object)             {
00099 throw(object); }
00100 #else
00101 template <typename T> static inline void  Raise(const T&)                           {}
00102 #endif
00103 
00104 
00105 
00106 //
00107 // GJK
00108 //
00109 struct GJK
00110        {
00111        struct Mkv
00112               {
00113               Vector3       w;            /* Minkowski vertice */
00114               Vector3       r;            /* Ray                             */
00115               };
00116        struct He
00117               {
00118               Vector3       v;
00119               He*           n;
00120               };
00121        btStackAlloc*                      sa;
00122        btBlock*             sablock;
00123        He*                                       table[GJK_hashsize];
00124        Rotation                           wrotations[2];
00125        Vector3                                   positions[2];
00126        const btConvexShape* shapes[2];
00127        Mkv                                       simplex[5];
00128        Vector3                                   ray;
00129        U                                         order;
00130        U                                         iterations;
00131        F                                         margin;
00132        Z                                         failed;
00133        //
00134                                    GJK(btStackAlloc* psa,
00135                                           const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0,
00136                                           const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1,
00137                                           F pmargin=0)
00138               {
00139               wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0;
00140               wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1;
00141               sa            =psa;
00142               sablock       =sa->beginBlock();
00143               margin =pmargin;
00144               failed =false;
00145               }
00146        //
00147                                    ~GJK()
00148               {
00149               sa->endBlock(sablock);
00150               }
00151        // vdh : very dumm hash
00152        static inline U      Hash(const Vector3& v)
00153               {
00154               //this doesn't compile under GCC 3.3.5, so add the ()...
00155               //const U     h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473));
00156               //return(((*((const U*)&h))*169639)&GJK_hashmask);
00157            const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473));
00158            return(((*((const U*)&h))*169639)&GJK_hashmask);
00159               }
00160        //
00161        inline Vector3       LocalSupport(const Vector3& d,U i) const
00162               {
00163               return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]);
00164               }
00165        //
00166        inline void          Support(const Vector3& d,Mkv& v) const
00167               {
00168               v.r    =      d;
00169               v.w    =      LocalSupport(d,0)-LocalSupport(-d,1)+d*margin;
00170               }
00171        #define SPX(_i_)     simplex[_i_]
00172        #define SPXW(_i_)    simplex[_i_].w
00173        //
00174        inline Z             FetchSupport()
00175               {
00176               const U                     h(Hash(ray));
00177               He*                         e = (He*)(table[h]);
00178               while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; }
00179               e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e;
00180               Support(ray,simplex[++order]);
00181               return(ray.dot(SPXW(order))>0);
00182               }
00183        //
00184        inline Z             SolveSimplex2(const Vector3& ao,const Vector3& ab)
00185               {
00186               if(ab.dot(ao)>=0)
00187                      {
00188                      const Vector3 cabo(cross(ab,ao));
00189                      if(cabo.length2()>GJK_sqinsimplex_eps)
00190                             { ray=cross(cabo,ab); }
00191                             else
00192                             { return(true); }
00193                      }
00194                      else
00195                      { order=0;SPX(0)=SPX(1);ray=ao;    }
00196               return(false);
00197               }
00198        //
00199        inline Z             SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& 
00200 ac)
00201               {
00202               return(SolveSimplex3a(ao,ab,ac,cross(ab,ac)));
00203               }
00204        //
00205        inline Z             SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& 
00206 ac,const Vector3& cabc)
00207               {
00208                             if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps)
00209                      { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab));   }
00210               else   if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps)
00211                      { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); }
00212               else
00213                      {
00214                      const F                     d(cabc.dot(ao));
00215                      if(Abs(d)>GJK_insimplex_eps)
00216                             {
00217                             if(d>0)
00218                                    { ray=cabc; }
00219                                    else
00220                                    { ray=-cabc;Swap(SPX(0),SPX(1)); }
00221                             return(false);
00222                             } else return(true);
00223                      }
00224               }
00225        //
00226        inline Z             SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& 
00227 ac,const Vector3& ad)
00228               {
00229               Vector3                     crs;
00230                             if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps)
00231                      { 
00232 order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); 
00233 }
00234               else   if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps)
00235                      { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); }
00236               else   if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps)
00237                      { 
00238 order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); 
00239 }
00240               else   return(true);
00241               }
00242        //
00243        inline Z             SearchOrigin(const Vector3& initray=Vector3(1,0,0))
00244               {
00245               iterations    =      0;
00246               order         =      (U)-1;
00247               failed        =      false;
00248               ray                  =      initray.normalized();
00249               ClearMemory(table,sizeof(void*)*GJK_hashsize);
00250               FetchSupport();
00251               ray                  =      -SPXW(0);
00252               for(;iterations<GJK_maxiterations;++iterations)
00253                      {
00254                      const F              rl(ray.length());
00255                      ray/=rl>0?rl:1;
00256                      if(FetchSupport())
00257                             {
00258                             Z      found(false);
00259                             switch(order)
00260                                    {
00261                                    case   1:     found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break;
00262                                    case   2:     found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break;
00263                                    case   3:     found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break;
00264                                    }
00265                             if(found) return(true);
00266                             } else return(false);
00267                      }
00268               failed=true;
00269               return(false);
00270               }
00271        //
00272        inline Z             EncloseOrigin()
00273               {
00274               switch(order)
00275                      {
00276                      /* Point             */
00277                      case   0:     break;
00278                      /* Line                     */
00279                      case   1:
00280                             {
00281                             const Vector3 ab(SPXW(1)-SPXW(0));
00282                             const Vector3 b[]={  cross(ab,Vector3(1,0,0)),
00283                                                                       cross(ab,Vector3(0,1,0)),
00284                                                                       cross(ab,Vector3(0,0,1))};
00285                             const F                     m[]={b[0].length2(),b[1].length2(),b[2].length2()};
00286                             const Rotation       r(btQuaternion(ab.normalized(),cst2Pi/3));
00287                             Vector3                     w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]);
00288                             Support(w.normalized(),simplex[4]);w=r*w;
00289                             Support(w.normalized(),simplex[2]);w=r*w;
00290                             Support(w.normalized(),simplex[3]);w=r*w;
00291                             order=4;
00292                             return(true);
00293                             }
00294                      break;
00295                      /* Triangle          */
00296                      case   2:
00297                             {
00298                             const 
00299 Vector3       n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized());
00300                             Support( n,simplex[3]);
00301                             Support(-n,simplex[4]);
00302                             order=4;
00303                             return(true);
00304                             }
00305                      break;
00306                      /* Tetrahedron       */
00307                      case   3:     return(true);
00308                      /* Hexahedron */
00309                      case   4:     return(true);
00310                      }
00311               return(false);
00312               }
00313        #undef SPX
00314        #undef SPXW
00315        };
00316 
00317 //
00318 // EPA
00319 //
00320 struct EPA
00321        {
00322        //
00323        struct Face
00324               {
00325               const GJK::Mkv*      v[3];
00326               Face*                f[3];
00327               U                           e[3];
00328               Vector3                     n;
00329               F                           d;
00330               U                           mark;
00331               Face*                prev;
00332               Face*                next;
00333               Face()               {}
00334               };
00335        //
00336        GJK*                 gjk;
00337        btStackAlloc*        sa;
00338        Face*                root;
00339        U                           nfaces;
00340        U                           iterations;
00341        Vector3                     features[2][3];
00342        Vector3                     nearest[2];
00343        Vector3                     normal;
00344        F                           depth;
00345        Z                           failed;
00346        //
00347                                    EPA(GJK* pgjk)
00348               {
00349               gjk           =      pgjk;
00350               sa            =      pgjk->sa;
00351               }
00352        //
00353                                    ~EPA()
00354               {
00355               }
00356        //
00357        inline Vector3       GetCoordinates(const Face* face) const
00358               {
00359               const Vector3 o(face->n*-face->d);
00360               const F                     a[]={  cross(face->v[0]->w-o,face->v[1]->w-o).length(),
00361                                                         cross(face->v[1]->w-o,face->v[2]->w-o).length(),
00362                                                         cross(face->v[2]->w-o,face->v[0]->w-o).length()};
00363               const F                     sm(a[0]+a[1]+a[2]);
00364               return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1));
00365               }
00366        //
00367        inline Face*  FindBest() const
00368               {
00369               Face*  bf = 0;
00370               if(root)
00371                      {
00372                      Face*  cf = root;
00373                      F             bd(cstInf);
00374                      do     {
00375                             if(cf->d<bd) { bd=cf->d;bf=cf; }
00376                             } while(0!=(cf=cf->next));
00377                      }
00378               return(bf);
00379               }
00380        //
00381        inline Z             Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv*
00382 c) const
00383               {
00384               const Vector3 nrm(cross(b->w-a->w,c->w-a->w));
00385               const F                     len(nrm.length());
00386               const Z                     valid( (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&&
00387                                                         (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&&
00388                                                         (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps));
00389               f->v[0]       =      a;
00390               f->v[1]       =      b;
00391               f->v[2]       =      c;
00392               f->mark       =      0;
00393               f->n   =      nrm/(len>0?len:cstInf);
00394               f->d   =      Max<F>(0,-f->n.dot(a->w));
00395               return(valid);
00396               }
00397        //
00398        inline Face*  NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
00399               {
00400               Face*  pf = (Face*)sa->allocate(sizeof(Face));
00401               if(Set(pf,a,b,c))
00402                      {
00403                      if(root) root->prev=pf;
00404                      pf->prev=0;
00405                      pf->next=root;
00406                      root   =pf;
00407                      ++nfaces;
00408                      }
00409                      else
00410                      {
00411                      pf->prev=pf->next=0;
00412                      }
00413               return(pf);
00414               }
00415        //
00416        inline void          Detach(Face* face)
00417               {
00418               if(face->prev||face->next)
00419                      {
00420                      --nfaces;
00421                      if(face==root)
00422                             { root=face->next;root->prev=0; }
00423                             else
00424                             {
00425                             if(face->next==0)
00426                                    { face->prev->next=0; }
00427                                    else
00428                                    { face->prev->next=face->next;face->next->prev=face->prev; }
00429                             }
00430                      face->prev=face->next=0;
00431                      }
00432               }
00433        //
00434        inline void          Link(Face* f0,U e0,Face* f1,U e1) const
00435               {
00436               f0->f[e0]=f1;f1->e[e1]=e0;
00437               f1->f[e1]=f0;f0->e[e0]=e1;
00438               }
00439        //
00440        GJK::Mkv*            Support(const Vector3& w) const
00441               {
00442               GJK::Mkv*            v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));
00443               gjk->Support(w,*v);
00444               return(v);
00445               }
00446        //
00447        U                           BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& 
00448 ff)
00449               {
00450               static const U       mod3[]={0,1,2,0,1};
00451               U                           ne(0);
00452               if(f.mark!=markid)
00453                      {
00454                      const U       e1(mod3[e+1]);
00455                      if((f.n.dot(w->w)+f.d)>0)
00456                             {
00457                             Face*  nf = NewFace(f.v[e1],f.v[e],w);
00458                             Link(nf,0,&f,e);
00459                             if(cf) Link(cf,1,nf,2); else ff=nf;
00460                             cf=nf;ne=1;
00461                             }
00462                             else
00463                             {
00464                             const U       e2(mod3[e+2]);
00465                             Detach(&f);
00466                             f.mark =      markid;
00467                             ne            +=     BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff);
00468                             ne            +=     BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff);
00469                             }
00470                      }
00471               return(ne);
00472               }
00473        //
00474        inline F             EvaluatePD(F accuracy=EPA_accuracy)
00475               {
00476               btBlock*      sablock = sa->beginBlock();
00477               Face*                       bestface = 0;
00478               U                                  markid(1);
00479               depth         =      -cstInf;
00480               normal        =      Vector3(0,0,0);
00481               root          =      0;
00482               nfaces        =      0;
00483               iterations    =      0;
00484               failed        =      false;
00485               /* Prepare hull             */
00486               if(gjk->EncloseOrigin())
00487                      {
00488                      const U*             pfidx = 0;
00489                      U                           nfidx= 0;
00490                      const U*             peidx = 0;
00491                      U                           neidx = 0;
00492                      GJK::Mkv*            basemkv[5];
00493                      Face*                basefaces[6];
00494                      switch(gjk->order)
00495                             {
00496                             /* Tetrahedron              */
00497                             case   3:     {
00498                                                  static const U       fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}};
00499                                                  static const 
00500 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}};
00501                                                  pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6;
00502                                                  }      break;
00503                             /* Hexahedron        */
00504                             case   4:     {
00505                                                  static const 
00506 U      fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}};
00507                                                  static const 
00508 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}};
00509                                                  pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9;
00510                                                  }      break;
00511                             }
00512                      U i;
00513 
00514                      for( i=0;i<=gjk->order;++i)        { 
00515 basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; 
00516 }
00517                      for( i=0;i<nfidx;++i,pfidx+=3)            { 
00518 basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); 
00519 }
00520                      for( i=0;i<neidx;++i,peidx+=4)            { 
00521 Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
00522                      }
00523               if(0==nfaces)
00524                      {
00525                      sa->endBlock(sablock);
00526                      return(depth);
00527                      }
00528               /* Expand hull              */
00529               for(;iterations<EPA_maxiterations;++iterations)
00530                      {
00531                      Face*         bf = FindBest();
00532                      if(bf)
00533                             {
00534                             GJK::Mkv*     w = Support(-bf->n);
00535                             const F              d(bf->n.dot(w->w)+bf->d);
00536                             bestface      =      bf;
00537                             if(d<-accuracy)
00538                                    {
00539                                    Face*  cf =0;
00540                                    Face*  ff =0;
00541                                    U             nf = 0;
00542                                    Detach(bf);
00543                                    bf->mark=++markid;
00544                                    for(U i=0;i<3;++i)   { 
00545 nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
00546                                    if(nf<=2)                   { break; }
00547                                    Link(cf,1,ff,2);
00548                                    } else break;
00549                             } else break;
00550                      }
00551               /* Extract contact   */
00552               if(bestface)
00553                      {
00554                      const Vector3 b(GetCoordinates(bestface));
00555                      normal               =      bestface->n;
00556                      depth                =      Max<F>(0,bestface->d);
00557                      for(U i=0;i<2;++i)
00558                             {
00559                             const F       s(F(i?-1:1));
00560                             for(U j=0;j<3;++j)
00561                                    {
00562                                    features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i);
00563                                    }
00564                             }
00565                      nearest[0]           =      features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z();
00566                      nearest[1]           =      features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z();
00567                      } else failed=true;
00568               sa->endBlock(sablock);
00569               return(depth);
00570               }
00571        };
00572 }
00573 
00574 //
00575 // Api
00576 //
00577 
00578 using namespace      gjkepa_impl;
00579 
00580 
00581 
00582 //
00583 bool   btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0,
00584                                                         const btConvexShape *shape1,const btTransform &wtrs1,
00585                                                         btScalar      radialmargin,
00586                                                         btStackAlloc* stackAlloc,
00587                                                         sResults&     results)
00588 {
00589        
00590 
00591 /* Initialize                             */
00592 results.witnesses[0] =
00593 results.witnesses[1] =
00594 results.normal                     =      Vector3(0,0,0);
00595 results.depth               =      0;
00596 results.status                     =      sResults::Separated;
00597 results.epa_iterations      =      0;
00598 results.gjk_iterations      =      0;
00599 /* Use GJK to locate origin        */
00600 GJK                  gjk(stackAlloc,
00601                             wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
00602                             wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
00603                             radialmargin+EPA_accuracy);
00604 const Z              collide(gjk.SearchOrigin());
00605 results.gjk_iterations      =      gjk.iterations+1;
00606 if(collide)
00607        {
00608        /* Then EPA for penetration depth  */
00609        EPA                  epa(&gjk);
00610        const F              pd(epa.EvaluatePD());
00611        results.epa_iterations      =      epa.iterations+1;
00612        if(pd>0)
00613               {
00614               results.status                     =      sResults::Penetrating;
00615               results.normal                     =      epa.normal;
00616               results.depth               =      pd;
00617               results.witnesses[0] =      epa.nearest[0];
00618               results.witnesses[1] =      epa.nearest[1];
00619               return(true);
00620               } else { if(epa.failed) results.status=sResults::EPA_Failed; }
00621        } else { if(gjk.failed) results.status=sResults::GJK_Failed; }
00622 return(false);
00623 }
00624 
00625 
00626 
00627 
00628