btGjkEpa2.cpp

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