Back to index

supertuxkart  0.5+dfsg1
btGjkEpa2.cpp
Go to the documentation of this file.
00001 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
00002 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00003 #include "btGjkEpa2.h"
00004 
00005 #if defined(DEBUG) || defined (_DEBUG)
00006 #include <stdio.h> //for debug printf
00007 #ifdef __SPU__
00008 #include <spu_printf.h>
00009 #define printf spu_printf
00010 #endif //__SPU__
00011 #endif
00012 
00013 namespace gjkepa2_impl
00014 {
00015 
00016 // Config
00017 
00018        /* GJK */ 
00019 #define GJK_MAX_ITERATIONS  128
00020 #define GJK_ACCURARY        ((btScalar)0.0001)
00021 #define GJK_MIN_DISTANCE    ((btScalar)0.0001)
00022 #define GJK_DUPLICATED_EPS  ((btScalar)0.0001)
00023 #define GJK_SIMPLEX2_EPS    ((btScalar)0.0)
00024 #define GJK_SIMPLEX3_EPS    ((btScalar)0.0)
00025 #define GJK_SIMPLEX4_EPS    ((btScalar)0.0)
00026 
00027        /* EPA */ 
00028 #define EPA_MAX_VERTICES    64
00029 #define EPA_MAX_FACES              (EPA_MAX_VERTICES*2)
00030 #define EPA_MAX_ITERATIONS  255
00031 #define EPA_ACCURACY        ((btScalar)0.0001)
00032 #define EPA_FALLBACK        (10*EPA_ACCURACY)
00033 #define EPA_PLANE_EPS              ((btScalar)0.00001)
00034 #define EPA_INSIDE_EPS             ((btScalar)0.01)
00035 
00036 
00037 // Shorthands
00038 typedef unsigned int U;
00039 typedef unsigned char       U1;
00040 
00041 // MinkowskiDiff
00042 struct MinkowskiDiff
00043        {
00044        const btConvexShape* m_shapes[2];
00045        btMatrix3x3                        m_toshape1;
00046        btTransform                        m_toshape0;
00047        btVector3                          (btConvexShape::*Ls)(const btVector3&) const;
00048        void                               EnableMargin(bool enable)
00049               {
00050               if(enable)
00051                      Ls=&btConvexShape::localGetSupportingVertex;
00052                      else
00053                      Ls=&btConvexShape::localGetSupportingVertexWithoutMargin;
00054               }      
00055        inline btVector3            Support0(const btVector3& d) const
00056               {
00057               return(((m_shapes[0])->*(Ls))(d));
00058               }
00059        inline btVector3            Support1(const btVector3& d) const
00060               {
00061               return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
00062               }
00063        inline btVector3            Support(const btVector3& d) const
00064               {
00065               return(Support0(d)-Support1(-d));
00066               }
00067        btVector3                          Support(const btVector3& d,U index) const
00068               {
00069               if(index)
00070                      return(Support1(d));
00071                      else
00072                      return(Support0(d));
00073               }
00074        };
00075 
00076 typedef       MinkowskiDiff tShape;
00077 
00078 // GJK
00079 struct GJK
00080 {
00081 /* Types             */ 
00082 struct sSV
00083        {
00084        btVector3     d,w;
00085        };
00086 struct sSimplex
00087        {
00088        sSV*          c[4];
00089        btScalar      p[4];
00090        U                    rank;
00091        };
00092 struct eStatus       { enum _ {
00093        Valid,
00094        Inside,
00095        Failed        };};
00096 /* Fields            */ 
00097 tShape               m_shape;
00098 btVector3            m_ray;
00099 btScalar             m_distance;
00100 sSimplex             m_simplices[2];
00101 sSV                         m_store[4];
00102 sSV*                 m_free[4];
00103 U                           m_nfree;
00104 U                           m_current;
00105 sSimplex*            m_simplex;
00106 eStatus::_           m_status;
00107 /* Methods           */ 
00108                                    GJK()
00109        {
00110        Initialize();
00111        }
00112 void                        Initialize()
00113        {
00114        m_ray         =      btVector3(0,0,0);
00115        m_nfree              =      0;
00116        m_status      =      eStatus::Failed;
00117        m_current     =      0;
00118        m_distance    =      0;
00119        }
00120 eStatus::_                  Evaluate(const tShape& shapearg,const btVector3& guess)
00121        {
00122        U                    iterations=0;
00123        btScalar      sqdist=0;
00124        btScalar      alpha=0;
00125        btVector3     lastw[4];
00126        U                    clastw=0;
00127        /* Initialize solver        */ 
00128        m_free[0]                   =      &m_store[0];
00129        m_free[1]                   =      &m_store[1];
00130        m_free[2]                   =      &m_store[2];
00131        m_free[3]                   =      &m_store[3];
00132        m_nfree                            =      4;
00133        m_current                   =      0;
00134        m_status                    =      eStatus::Valid;
00135        m_shape                            =      shapearg;
00136        m_distance                  =      0;
00137        /* Initialize simplex              */ 
00138        m_simplices[0].rank  =      0;
00139        m_ray                       =      guess;
00140        const btScalar       sqrl=  m_ray.length2();
00141        appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
00142        m_simplices[0].p[0]  =      1;
00143        m_ray                       =      m_simplices[0].c[0]->w;     
00144        sqdist                      =      sqrl;
00145        lastw[0]                    =
00146        lastw[1]                    =
00147        lastw[2]                    =
00148        lastw[3]                    =      m_ray;
00149        /* Loop                                          */ 
00150        do     {
00151               const U              next=1-m_current;
00152               sSimplex&     cs=m_simplices[m_current];
00153               sSimplex&     ns=m_simplices[next];
00154               /* Check zero                                           */ 
00155               const btScalar       rl=m_ray.length();
00156               if(rl<GJK_MIN_DISTANCE)
00157                      {/* Touching or inside                           */ 
00158                      m_status=eStatus::Inside;
00159                      break;
00160                      }
00161               /* Append new vertice in -'v' direction   */ 
00162               appendvertice(cs,-m_ray);
00163               const btVector3&     w=cs.c[cs.rank-1]->w;
00164               bool                        found=false;
00165               for(U i=0;i<4;++i)
00166                      {
00167                      if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
00168                             { found=true;break; }
00169                      }
00170               if(found)
00171                      {/* Return old simplex                           */ 
00172                      removevertice(m_simplices[m_current]);
00173                      break;
00174                      }
00175                      else
00176                      {/* Update lastw                                 */ 
00177                      lastw[clastw=(clastw+1)&3]=w;
00178                      }
00179               /* Check for termination                         */ 
00180               const btScalar       omega=dot(m_ray,w)/rl;
00181               alpha=btMax(omega,alpha);
00182               if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
00183                      {/* Return old simplex                           */ 
00184                      removevertice(m_simplices[m_current]);
00185                      break;
00186                      }             
00187               /* Reduce simplex                                       */ 
00188               btScalar      weights[4];
00189               U                    mask=0;
00190               switch(cs.rank)
00191                      {
00192                      case   2:     sqdist=projectorigin(       cs.c[0]->w,
00193                                                                                     cs.c[1]->w,
00194                                                                                     weights,mask);break;
00195                      case   3:     sqdist=projectorigin(       cs.c[0]->w,
00196                                                                                     cs.c[1]->w,
00197                                                                                     cs.c[2]->w,
00198                                                                                     weights,mask);break;
00199                      case   4:     sqdist=projectorigin(       cs.c[0]->w,
00200                                                                                     cs.c[1]->w,
00201                                                                                     cs.c[2]->w,
00202                                                                                     cs.c[3]->w,
00203                                                                                     weights,mask);break;
00204                      }
00205               if(sqdist>=0)
00206                      {/* Valid     */ 
00207                      ns.rank              =      0;
00208                      m_ray         =      btVector3(0,0,0);
00209                      m_current     =      next;
00210                      for(U i=0,ni=cs.rank;i<ni;++i)
00211                             {
00212                             if(mask&(1<<i))
00213                                    {
00214                                    ns.c[ns.rank]        =      cs.c[i];
00215                                    ns.p[ns.rank++]             =      weights[i];
00216                                    m_ray                       +=     cs.c[i]->w*weights[i];
00217                                    }
00218                                    else
00219                                    {
00220                                    m_free[m_nfree++]    =      cs.c[i];
00221                                    }
00222                             }
00223                      if(mask==15) m_status=eStatus::Inside;
00224                      }
00225                      else
00226                      {/* Return old simplex                           */ 
00227                      removevertice(m_simplices[m_current]);
00228                      break;
00229                      }
00230               m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
00231               } while(m_status==eStatus::Valid);
00232        m_simplex=&m_simplices[m_current];
00233        switch(m_status)
00234               {
00235               case   eStatus::Valid:             m_distance=m_ray.length();break;
00236               case   eStatus::Inside:     m_distance=0;break;
00237               }      
00238        return(m_status);
00239        }
00240 bool                               EncloseOrigin()
00241        {
00242        switch(m_simplex->rank)
00243               {
00244               case   1:
00245                      {
00246                      for(U i=0;i<3;++i)
00247                             {
00248                             btVector3            axis=btVector3(0,0,0);
00249                             axis[i]=1;
00250                             appendvertice(*m_simplex, axis);
00251                             if(EncloseOrigin())  return(true);
00252                             removevertice(*m_simplex);
00253                             appendvertice(*m_simplex,-axis);
00254                             if(EncloseOrigin())  return(true);
00255                             removevertice(*m_simplex);
00256                             }
00257                      }
00258               break;
00259               case   2:
00260                      {
00261                      const btVector3      d=m_simplex->c[1]->w-m_simplex->c[0]->w;
00262                      for(U i=0;i<3;++i)
00263                             {
00264                             btVector3            axis=btVector3(0,0,0);
00265                             axis[i]=1;
00266                             if(btFabs(dot(axis,d))>0)
00267                                    {
00268                                    const btVector3      p=cross(d,axis);
00269                                    appendvertice(*m_simplex, p);
00270                                    if(EncloseOrigin())  return(true);
00271                                    removevertice(*m_simplex);
00272                                    appendvertice(*m_simplex,-p);
00273                                    if(EncloseOrigin())  return(true);
00274                                    removevertice(*m_simplex);
00275                                    }
00276                             }
00277                      }
00278               break;
00279               case   3:
00280                      {
00281                      const btVector3      n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
00282                                                                m_simplex->c[2]->w-m_simplex->c[0]->w);
00283                      const btScalar       l=n.length();
00284                      if(l>0)
00285                             {
00286                             appendvertice(*m_simplex,n);
00287                             if(EncloseOrigin())  return(true);
00288                             removevertice(*m_simplex);
00289                             appendvertice(*m_simplex,-n);
00290                             if(EncloseOrigin())  return(true);
00291                             removevertice(*m_simplex);
00292                             }
00293                      }
00294               break;
00295               case   4:
00296                      {
00297                      if(btFabs(det(       m_simplex->c[0]->w-m_simplex->c[3]->w,
00298                                                  m_simplex->c[1]->w-m_simplex->c[3]->w,
00299                                                  m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
00300                             return(true);
00301                      }
00302               break;
00303               }
00304        return(false);
00305        }
00306 /* Internals  */ 
00307 void                        getsupport(const btVector3& d,sSV& sv) const
00308        {
00309        sv.d   =      d/d.length();
00310        sv.w   =      m_shape.Support(sv.d);
00311        }
00312 void                        removevertice(sSimplex& simplex)
00313        {
00314        m_free[m_nfree++]=simplex.c[--simplex.rank];
00315        }
00316 void                        appendvertice(sSimplex& simplex,const btVector3& v)
00317        {
00318        simplex.p[simplex.rank]=0;
00319        simplex.c[simplex.rank]=m_free[--m_nfree];
00320        getsupport(v,*simplex.c[simplex.rank++]);
00321        }
00322 static btScalar             det(const btVector3& a,const btVector3& b,const btVector3& c)
00323        {
00324        return(       a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
00325                      a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
00326                      a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
00327        }
00328 static btScalar             projectorigin(       const btVector3& a,
00329                                                                const btVector3& b,
00330                                                                btScalar* w,U& m)
00331        {
00332        const btVector3      d=b-a;
00333        const btScalar       l=d.length2();
00334        if(l>GJK_SIMPLEX2_EPS)
00335               {
00336               const btScalar       t(l>0?-dot(a,d)/l:0);
00337               if(t>=1)             { w[0]=0;w[1]=1;m=2;return(b.length2()); }
00338               else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
00339               else                 { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
00340               }
00341        return(-1);
00342        }
00343 static btScalar             projectorigin(       const btVector3& a,
00344                                                                const btVector3& b,
00345                                                                const btVector3& c,
00346                                                                btScalar* w,U& m)
00347        {
00348        static const U              imd3[]={1,2,0};
00349        const btVector3*     vt[]={&a,&b,&c};
00350        const btVector3             dl[]={a-b,b-c,c-a};
00351        const btVector3             n=cross(dl[0],dl[1]);
00352        const btScalar              l=n.length2();
00353        if(l>GJK_SIMPLEX3_EPS)
00354               {
00355               btScalar      mindist=-1;
00356               btScalar      subw[2];
00357               U                    subm;
00358               for(U i=0;i<3;++i)
00359                      {
00360                      if(dot(*vt[i],cross(dl[i],n))>0)
00361                             {
00362                             const U                     j=imd3[i];
00363                             const btScalar       subd(projectorigin(*vt[i],*vt[j],subw,subm));
00364                             if((mindist<0)||(subd<mindist))
00365                                    {
00366                                    mindist              =      subd;
00367                                    m                    =      ((subm&1)?1<<i:0)+((subm&2)?1<<j:0);
00368                                    w[i]          =      subw[0];
00369                                    w[j]          =      subw[1];
00370                                    w[imd3[j]]    =      0;                          
00371                                    }
00372                             }
00373                      }
00374               if(mindist<0)
00375                      {
00376                      const btScalar       d=dot(a,n);   
00377                      const btScalar       s=btSqrt(l);
00378                      const btVector3      p=n*(d/l);
00379                      mindist       =      p.length2();
00380                      m             =      7;
00381                      w[0]   =      (cross(dl[1],b-p)).length()/s;
00382                      w[1]   =      (cross(dl[2],c-p)).length()/s;
00383                      w[2]   =      1-(w[0]+w[1]);
00384                      }
00385               return(mindist);
00386               }
00387        return(-1);
00388        }
00389 static btScalar             projectorigin(       const btVector3& a,
00390                                                                const btVector3& b,
00391                                                                const btVector3& c,
00392                                                                const btVector3& d,
00393                                                                btScalar* w,U& m)
00394        {
00395        static const U              imd3[]={1,2,0};
00396        const btVector3*     vt[]={&a,&b,&c,&d};
00397        const btVector3             dl[]={a-d,b-d,c-d};
00398        const btScalar              vl=det(dl[0],dl[1],dl[2]);
00399        const bool                  ng=(vl*dot(a,cross(b-c,a-b)))<=0;
00400        if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
00401               {
00402               btScalar      mindist=-1;
00403               btScalar      subw[3];
00404               U                    subm;
00405               for(U i=0;i<3;++i)
00406                      {
00407                      const U                     j=imd3[i];
00408                      const btScalar       s=vl*dot(d,cross(dl[i],dl[j]));
00409                      if(s>0)
00410                             {
00411                             const btScalar       subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
00412                             if((mindist<0)||(subd<mindist))
00413                                    {
00414                                    mindist              =      subd;
00415                                    m                    =      (subm&1?1<<i:0)+
00416                                                                (subm&2?1<<j:0)+
00417                                                                (subm&4?8:0);
00418                                    w[i]          =      subw[0];
00419                                    w[j]          =      subw[1];
00420                                    w[imd3[j]]    =      0;
00421                                    w[3]          =      subw[2];
00422                                    }
00423                             }
00424                      }
00425               if(mindist<0)
00426                      {
00427                      mindist       =      0;
00428                      m             =      15;
00429                      w[0]   =      det(c,b,d)/vl;
00430                      w[1]   =      det(a,c,d)/vl;
00431                      w[2]   =      det(b,a,d)/vl;
00432                      w[3]   =      1-(w[0]+w[1]+w[2]);
00433                      }
00434               return(mindist);
00435               }
00436        return(-1);
00437        }
00438 };
00439 
00440 // EPA
00441 struct EPA
00442 {
00443 /* Types             */ 
00444 typedef       GJK::sSV      sSV;
00445 struct sFace
00446        {
00447        btVector3     n;
00448        btScalar      d;
00449        btScalar      p;
00450        sSV*          c[3];
00451        sFace*        f[3];
00452        sFace*        l[2];
00453        U1                   e[3];
00454        U1                   pass;
00455        };
00456 struct sList
00457        {
00458        sFace*        root;
00459        U                    count;
00460                             sList() : root(0),count(0)  {}
00461        };
00462 struct sHorizon
00463        {
00464        sFace*        cf;
00465        sFace*        ff;
00466        U                    nf;
00467                             sHorizon() : cf(0),ff(0),nf(0)     {}
00468        };
00469 struct eStatus { enum _ {
00470        Valid,
00471        Touching,
00472        Degenerated,
00473        NonConvex,
00474        InvalidHull,         
00475        OutOfFaces,
00476        OutOfVertices,
00477        AccuraryReached,
00478        FallBack,
00479        Failed,              };};
00480 /* Fields            */ 
00481 eStatus::_           m_status;
00482 GJK::sSimplex m_result;
00483 btVector3            m_normal;
00484 btScalar             m_depth;
00485 sSV                         m_sv_store[EPA_MAX_VERTICES];
00486 sFace                m_fc_store[EPA_MAX_FACES];
00487 U                           m_nextsv;
00488 sList                m_hull;
00489 sList                m_stock;
00490 /* Methods           */ 
00491                                    EPA()
00492        {
00493        Initialize(); 
00494        }
00495 void                        Initialize()
00496        {
00497        m_status      =      eStatus::Failed;
00498        m_normal      =      btVector3(0,0,0);
00499        m_depth              =      0;
00500        m_nextsv      =      0;
00501        for(U i=0;i<EPA_MAX_FACES;++i)
00502               {
00503               append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
00504               }
00505        }
00506 eStatus::_                  Evaluate(GJK& gjk,const btVector3& guess)
00507        {
00508        GJK::sSimplex&       simplex=*gjk.m_simplex;
00509        if((simplex.rank>1)&&gjk.EncloseOrigin())
00510               {
00511               /* Clean up                        */ 
00512               while(m_hull.root)
00513                      {
00514                      sFace* f(m_hull.root);
00515                      remove(m_hull,f);
00516                      append(m_stock,f);
00517                      }
00518               m_status      =      eStatus::Valid;
00519               m_nextsv      =      0;
00520               /* Orient simplex           */ 
00521               if(gjk.det(   simplex.c[0]->w-simplex.c[3]->w,
00522                                    simplex.c[1]->w-simplex.c[3]->w,
00523                                    simplex.c[2]->w-simplex.c[3]->w)<0)
00524                      {
00525                      btSwap(simplex.c[0],simplex.c[1]);
00526                      btSwap(simplex.p[0],simplex.p[1]);
00527                      }
00528               /* Build initial hull       */ 
00529               sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
00530                                           newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
00531                                           newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
00532                                           newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
00533               if(m_hull.count==4)
00534                      {
00535                      sFace*        best=findbest();
00536                      sFace         outer=*best;
00537                      U                    pass=0;
00538                      U                    iterations=0;
00539                      bind(tetra[0],0,tetra[1],0);
00540                      bind(tetra[0],1,tetra[2],0);
00541                      bind(tetra[0],2,tetra[3],0);
00542                      bind(tetra[1],1,tetra[3],2);
00543                      bind(tetra[1],2,tetra[2],1);
00544                      bind(tetra[2],2,tetra[3],1);
00545                      m_status=eStatus::Valid;
00546                      for(;iterations<EPA_MAX_ITERATIONS;++iterations)
00547                             {
00548                             if(m_nextsv<EPA_MAX_VERTICES)
00549                                    {      
00550                                    sHorizon             horizon;
00551                                    sSV*                 w=&m_sv_store[m_nextsv++];
00552                                    bool                 valid=true;                               
00553                                    best->pass    =      (U1)(++pass);
00554                                    gjk.getsupport(best->n,*w);
00555                                    const btScalar       wdist=dot(best->n,w->w)-best->d;
00556                                    if(wdist>EPA_ACCURACY)
00557                                           {
00558                                           for(U j=0;(j<3)&&valid;++j)
00559                                                  {
00560                                                  valid&=expand(       pass,w,
00561                                                                              best->f[j],best->e[j],
00562                                                                              horizon);
00563                                                  }
00564                                           if(valid&&(horizon.nf>=3))
00565                                                  {
00566                                                  bind(horizon.cf,1,horizon.ff,2);
00567                                                  remove(m_hull,best);
00568                                                  append(m_stock,best);
00569                                                  best=findbest();
00570                                                  if(best->p>=outer.p) outer=*best;
00571                                                  } else { m_status=eStatus::InvalidHull;break; }
00572                                           } else { m_status=eStatus::AccuraryReached;break; }
00573                                    } else { m_status=eStatus::OutOfVertices;break; }
00574                             }
00575                      const btVector3      projection=outer.n*outer.d;
00576                      m_normal      =      outer.n;
00577                      m_depth              =      outer.d;
00578                      m_result.rank =      3;
00579                      m_result.c[0] =      outer.c[0];
00580                      m_result.c[1] =      outer.c[1];
00581                      m_result.c[2] =      outer.c[2];
00582                      m_result.p[0] =      cross( outer.c[1]->w-projection,
00583                                                                       outer.c[2]->w-projection).length();
00584                      m_result.p[1] =      cross( outer.c[2]->w-projection,
00585                                                                       outer.c[0]->w-projection).length();
00586                      m_result.p[2] =      cross( outer.c[0]->w-projection,
00587                                                                       outer.c[1]->w-projection).length();
00588                      const btScalar       sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
00589                      m_result.p[0] /=     sum;
00590                      m_result.p[1] /=     sum;
00591                      m_result.p[2] /=     sum;
00592                      return(m_status);
00593                      }
00594               }
00595        /* Fallback          */ 
00596        m_status      =      eStatus::FallBack;
00597        m_normal      =      -guess;
00598        const btScalar       nl=m_normal.length();
00599        if(nl>0)
00600               m_normal      =      m_normal/nl;
00601               else
00602               m_normal      =      btVector3(1,0,0);
00603        m_depth       =      0;
00604        m_result.rank=1;
00605        m_result.c[0]=simplex.c[0];
00606        m_result.p[0]=1;     
00607        return(m_status);
00608        }
00609 sFace*                      newface(sSV* a,sSV* b,sSV* c,bool forced)
00610        {
00611        if(m_stock.root)
00612               {
00613               sFace* face=m_stock.root;
00614               remove(m_stock,face);
00615               append(m_hull,face);
00616               face->pass    =      0;
00617               face->c[0]    =      a;
00618               face->c[1]    =      b;
00619               face->c[2]    =      c;
00620               face->n              =      cross(b->w-a->w,c->w-a->w);
00621               const btScalar       l=face->n.length();
00622               const bool           v=l>EPA_ACCURACY;
00623               face->p              =      btMin(btMin(
00624                                                  dot(a->w,cross(face->n,a->w-b->w)),
00625                                                  dot(b->w,cross(face->n,b->w-c->w))),
00626                                                  dot(c->w,cross(face->n,c->w-a->w)))       /
00627                                                  (v?l:1);
00628               face->p              =      face->p>=-EPA_INSIDE_EPS?0:face->p;
00629               if(v)
00630                      {
00631                      face->d              =      dot(a->w,face->n)/l;
00632                      face->n              /=     l;
00633                      if(forced||(face->d>=-EPA_PLANE_EPS))
00634                             {
00635                             return(face);
00636                             } else m_status=eStatus::NonConvex;
00637                      } else m_status=eStatus::Degenerated;
00638               remove(m_hull,face);
00639               append(m_stock,face);
00640               return(0);
00641               }
00642        m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
00643        return(0);
00644        }
00645 sFace*                      findbest()
00646        {
00647        sFace*        minf=m_hull.root;
00648        btScalar      mind=minf->d*minf->d;
00649        btScalar      maxp=minf->p;
00650        for(sFace* f=minf->l[1];f;f=f->l[1])
00651               {
00652               const btScalar       sqd=f->d*f->d;
00653               if((f->p>=maxp)&&(sqd<mind))
00654                      {
00655                      minf=f;
00656                      mind=sqd;
00657                      maxp=f->p;
00658                      }
00659               }
00660        return(minf);
00661        }
00662 bool                        expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
00663        {
00664        static const U       i1m3[]={1,2,0};
00665        static const U       i2m3[]={2,0,1};
00666        if(f->pass!=pass)
00667               {
00668               const U       e1=i1m3[e];
00669               if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
00670                      {
00671                      sFace* nf=newface(f->c[e1],f->c[e],w,false);
00672                      if(nf)
00673                             {
00674                             bind(nf,0,f,e);
00675                             if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
00676                             horizon.cf=nf;
00677                             ++horizon.nf;
00678                             return(true);
00679                             }
00680                      }
00681                      else
00682                      {
00683                      const U       e2=i2m3[e];
00684                      f->pass              =      (U1)pass;
00685                      if(    expand(pass,w,f->f[e1],f->e[e1],horizon)&&
00686                             expand(pass,w,f->f[e2],f->e[e2],horizon))
00687                             {
00688                             remove(m_hull,f);
00689                             append(m_stock,f);
00690                             return(true);
00691                             }
00692                      }
00693               }
00694        return(false);
00695        }
00696 static inline void          bind(sFace* fa,U ea,sFace* fb,U eb)
00697        {
00698        fa->e[ea]=(U1)eb;fa->f[ea]=fb;
00699        fb->e[eb]=(U1)ea;fb->f[eb]=fa;
00700        }
00701 static inline void          append(sList& list,sFace* face)
00702        {
00703        face->l[0]    =      0;
00704        face->l[1]    =      list.root;
00705        if(list.root) list.root->l[0]=face;
00706        list.root     =      face;
00707        ++list.count;
00708        }
00709 static inline void          remove(sList& list,sFace* face)
00710        {
00711        if(face->l[1]) face->l[1]->l[0]=face->l[0];
00712        if(face->l[0]) face->l[0]->l[1]=face->l[1];
00713        if(face==list.root) list.root=face->l[1];
00714        --list.count;
00715        }
00716 };
00717 
00718 //
00719 static void   Initialize(   const btConvexShape* shape0,const btTransform& wtrs0,
00720                                           const btConvexShape* shape1,const btTransform& wtrs1,
00721                                           btGjkEpaSolver2::sResults& results,
00722                                           tShape& shape,
00723                                           bool withmargins)
00724 {
00725 /* Results           */ 
00726 results.witnesses[0] =
00727 results.witnesses[1] =      btVector3(0,0,0);
00728 results.status                     =      btGjkEpaSolver2::sResults::Separated;
00729 /* Shape             */ 
00730 shape.m_shapes[0]           =      shape0;
00731 shape.m_shapes[1]           =      shape1;
00732 shape.m_toshape1            =      wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
00733 shape.m_toshape0            =      wtrs0.inverseTimes(wtrs1);
00734 shape.EnableMargin(withmargins);
00735 }
00736 
00737 }
00738 
00739 //
00740 // Api
00741 //
00742 
00743 using namespace      gjkepa2_impl;
00744 
00745 //
00746 int                  btGjkEpaSolver2::StackSizeRequirement()
00747 {
00748 return(sizeof(GJK)+sizeof(EPA));
00749 }
00750 
00751 //
00752 btScalar      btGjkEpaSolver2::Distance(  const btConvexShape* shape0,
00753                                                                       const btTransform&          wtrs0,
00754                                                                       const btConvexShape* shape1,
00755                                                                       const btTransform&          wtrs1,
00756                                                                       sResults&                          results)
00757 {
00758 tShape               shape;
00759 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
00760 GJK                         gjk;   
00761 GJK::eStatus::_      gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
00762 if(gjk_status==GJK::eStatus::Valid)
00763        {
00764        btVector3     w0=btVector3(0,0,0);
00765        btVector3     w1=btVector3(0,0,0);
00766        for(U i=0;i<gjk.m_simplex->rank;++i)
00767               {
00768               const btScalar       p=gjk.m_simplex->p[i];
00769               w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
00770               w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
00771               }
00772        results.witnesses[0] =      wtrs0*w0;
00773        results.witnesses[1] =      wtrs0*w1;
00774        return((w0-w1).length());
00775        }
00776        else
00777        {
00778        results.status       =      gjk_status==GJK::eStatus::Inside?
00779                                                  sResults::Penetrating       :
00780                                                  sResults::GJK_Failed ;
00781        return(-1);
00782        }
00783 }
00784 
00785 //
00786 btScalar      btGjkEpaSolver2::SignedDistance(const btVector3& position,
00787                                                                              btScalar margin,
00788                                                                              const btConvexShape* shape0,
00789                                                                              const btTransform& wtrs0,
00790                                                                              sResults& results)
00791 {
00792 tShape               shape;
00793 btSphereShape shape1(margin);
00794 btTransform          wtrs1(btQuaternion(0,0,0,1),position);
00795 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
00796 GJK                         gjk;   
00797 GJK::eStatus::_      gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
00798 if(gjk_status==GJK::eStatus::Valid)
00799        {
00800        btVector3     w0=btVector3(0,0,0);
00801        btVector3     w1=btVector3(0,0,0);
00802        for(U i=0;i<gjk.m_simplex->rank;++i)
00803               {
00804               const btScalar       p=gjk.m_simplex->p[i];
00805               w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
00806               w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
00807               }
00808        results.witnesses[0] =      wtrs0*w0;
00809        results.witnesses[1] =      wtrs0*w1;
00810        const btVector3      delta= results.witnesses[1]-
00811                                                  results.witnesses[0];
00812        const btScalar       margin=       shape0->getMargin()+
00813                                                  shape1.getMargin();
00814        const btScalar       length=       delta.length();      
00815        results.normal                     =      delta/length;
00816        results.witnesses[0] +=     results.normal*margin;
00817        return(length-margin);
00818        }
00819        else
00820        {
00821        if(gjk_status==GJK::eStatus::Inside)
00822               {
00823               if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
00824                      {
00825                      const btVector3      delta= results.witnesses[0]-
00826                                                                results.witnesses[1];
00827                      const btScalar       length=       delta.length();
00828                      results.normal       =      delta/length;               
00829                      return(-length);
00830                      }
00831               }      
00832        }
00833 return(SIMD_INFINITY);
00834 }
00835 
00836 //
00837 bool   btGjkEpaSolver2::Penetration(      const btConvexShape* shape0,
00838                                                                       const btTransform&          wtrs0,
00839                                                                       const btConvexShape* shape1,
00840                                                                       const btTransform&          wtrs1,
00841                                                                       const btVector3&            guess,
00842                                                                       sResults&                          results)
00843 {
00844 tShape               shape;
00845 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,true);
00846 GJK                         gjk;   
00847 GJK::eStatus::_      gjk_status=gjk.Evaluate(shape,-guess);
00848 switch(gjk_status)
00849        {
00850        case   GJK::eStatus::Inside:
00851               {
00852               EPA                         epa;
00853               EPA::eStatus::_      epa_status=epa.Evaluate(gjk,-guess);
00854               if(epa_status!=EPA::eStatus::Failed)
00855                      {
00856                      btVector3     w0=btVector3(0,0,0);
00857                      for(U i=0;i<epa.m_result.rank;++i)
00858                             {
00859                             w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
00860                             }
00861                      results.status                     =      sResults::Penetrating;
00862                      results.witnesses[0] =      wtrs0*w0;
00863                      results.witnesses[1] =      wtrs0*(w0-epa.m_normal*epa.m_depth);
00864                      return(true);
00865                      } else results.status=sResults::EPA_Failed;
00866               }
00867        break;
00868        case   GJK::eStatus::Failed:
00869        results.status=sResults::GJK_Failed;
00870        break;
00871        }
00872 return(false);
00873 }
00874 
00875 /* Symbols cleanup          */ 
00876 
00877 #undef GJK_MAX_ITERATIONS
00878 #undef GJK_ACCURARY
00879 #undef GJK_MIN_DISTANCE
00880 #undef GJK_DUPLICATED_EPS
00881 #undef GJK_SIMPLEX2_EPS
00882 #undef GJK_SIMPLEX3_EPS
00883 #undef GJK_SIMPLEX4_EPS
00884 
00885 #undef EPA_MAX_VERTICES
00886 #undef EPA_MAX_FACES
00887 #undef EPA_MAX_ITERATIONS
00888 #undef EPA_ACCURACY
00889 #undef EPA_FALLBACK
00890 #undef EPA_PLANE_EPS
00891 #undef EPA_INSIDE_EPS