src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the
   7 use of this software.
   8 Permission is granted to anyone to use this software for any purpose,
   9 including commercial applications, and to alter it and redistribute it
  10 freely,
  11 subject to the following restrictions:
  12
  13 1. The origin of this software must not be misrepresented; you must not
  14 claim that you wrote the original software. If you use this software in a
  15 product, an acknowledgment in the product documentation would be appreciated
  16 but is not required.
  17 2. Altered source versions must be plainly marked as such, and must not be
  18 misrepresented as being the original software.
  19 3. This notice may not be removed or altered from any source distribution.
  20 */
  21
  22 /*
  23 GJK-EPA collision solver by Nathanael Presson, 2008
  24 */
  25 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
  26 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  27 #include "btGjkEpa2.h"
  28
  29 #if defined(DEBUG) || defined (_DEBUG)
  30 #include <stdio.h> //for debug printf
  31 #ifdef __SPU__
  32 #include <spu_printf.h>
  33 #define printf spu_printf
  34 #endif //__SPU__
  35 #endif
  36
  37 namespace gjkepa2_impl
  38 {
  39
  40         // Config
  41
  42         /* GJK  */
  43 #define GJK_MAX_ITERATIONS      128
  44 #define GJK_ACCURARY            ((btScalar)0.0001)
  45 #define GJK_MIN_DISTANCE        ((btScalar)0.0001)
  46 #define GJK_DUPLICATED_EPS      ((btScalar)0.0001)
  47 #define GJK_SIMPLEX2_EPS        ((btScalar)0.0)
  48 #define GJK_SIMPLEX3_EPS        ((btScalar)0.0)
  49 #define GJK_SIMPLEX4_EPS        ((btScalar)0.0)
  50
  51         /* EPA  */
  52 #define EPA_MAX_VERTICES        64
  53 #define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)
  54 #define EPA_MAX_ITERATIONS      255
  55 #define EPA_ACCURACY            ((btScalar)0.0001)
  56 #define EPA_FALLBACK            (10*EPA_ACCURACY)
  57 #define EPA_PLANE_EPS           ((btScalar)0.00001)
  58 #define EPA_INSIDE_EPS          ((btScalar)0.01)
  59
  60
  61         // Shorthands
  62         typedef unsigned int    U;
  63         typedef unsigned char   U1;
  64
  65         // MinkowskiDiff
  66         struct  MinkowskiDiff
  67         {
  68                 const btConvexShape*    m_shapes[2];
  69                 btMatrix3x3                             m_toshape1;
  70                 btTransform                             m_toshape0;
  71 #ifdef __SPU__
  72                 bool                                    m_enableMargin;
  73 #else
  74                 btVector3                               (btConvexShape::*Ls)(const btVector3&) const;
  75 #endif//__SPU__
  76
  77
  78                 MinkowskiDiff()
  79                 {
  80
  81                 }
  82 #ifdef __SPU__
  83                         void                                    EnableMargin(bool enable)
  84                 {
  85                         m_enableMargin = enable;
  86                 }
  87                 inline btVector3                Support0(const btVector3& d) const
  88                 {
  89                         if (m_enableMargin)
  90                         {
  91                                 return m_shapes[0]->localGetSupportVertexNonVirtual(d);
  92                         } else
  93                         {
  94                                 return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
  95                         }
  96                 }
  97                 inline btVector3                Support1(const btVector3& d) const
  98                 {
  99                         if (m_enableMargin)
 100                         {
 101                                 return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
 102                         } else
 103                         {
 104                                 return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
 105                         }
 106                 }
 107 #else
 108                 void                                    EnableMargin(bool enable)
 109                 {
 110                         if(enable)
 111                                 Ls=&btConvexShape::localGetSupportVertexNonVirtual;
 112                         else
 113                                 Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
 114                 }
 115                 inline btVector3                Support0(const btVector3& d) const
 116                 {
 117                         return(((m_shapes[0])->*(Ls))(d));
 118                 }
 119                 inline btVector3                Support1(const btVector3& d) const
 120                 {
 121                         return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
 122                 }
 123 #endif //__SPU__
 124
 125                 inline btVector3                Support(const btVector3& d) const
 126                 {
 127                         return(Support0(d)-Support1(-d));
 128                 }
 129                 btVector3                               Support(const btVector3& d,U index) const
 130                 {
 131                         if(index)
 132                                 return(Support1(d));
 133                         else
 134                                 return(Support0(d));
 135                 }
 136         };
 137
 138         typedef MinkowskiDiff   tShape;
 139
 140
 141         // GJK
 142         struct  GJK
 143         {
 144                 /* Types                */
 145                 struct  sSV
 146                 {
 147                         btVector3       d,w;
 148                 };
 149                 struct  sSimplex
 150                 {
 151                         sSV*            c[4];
 152                         btScalar        p[4];
 153                         U                       rank;
 154                 };
 155                 struct  eStatus { enum _ {
 156                         Valid,
 157                         Inside,
 158                         Failed          };};
 159                         /* Fields               */
 160                         tShape                  m_shape;
 161                         btVector3               m_ray;
 162                         btScalar                m_distance;
 163                         sSimplex                m_simplices[2];
 164                         sSV                             m_store[4];
 165                         sSV*                    m_free[4];
 166                         U                               m_nfree;
 167                         U                               m_current;
 168                         sSimplex*               m_simplex;
 169                         eStatus::_              m_status;
 170                         /* Methods              */
 171                         GJK()
 172                         {
 173                                 Initialize();
 174                         }
 175                         void                            Initialize()
 176                         {
 177                                 m_ray           =       btVector3(0,0,0);
 178                                 m_nfree         =       0;
 179                                 m_status        =       eStatus::Failed;
 180                                 m_current       =       0;
 181                                 m_distance      =       0;
 182                         }
 183                         eStatus::_                      Evaluate(const tShape& shapearg,const btVector3& guess)
 184                         {
 185                                 U                       iterations=0;
 186                                 btScalar        sqdist=0;
 187                                 btScalar        alpha=0;
 188                                 btVector3       lastw[4];
 189                                 U                       clastw=0;
 190                                 /* Initialize solver            */
 191                                 m_free[0]                       =       &m_store[0];
 192                                 m_free[1]                       =       &m_store[1];
 193                                 m_free[2]                       =       &m_store[2];
 194                                 m_free[3]                       =       &m_store[3];
 195                                 m_nfree                         =       4;
 196                                 m_current                       =       0;
 197                                 m_status                        =       eStatus::Valid;
 198                                 m_shape                         =       shapearg;
 199                                 m_distance                      =       0;
 200                                 /* Initialize simplex           */
 201                                 m_simplices[0].rank     =       0;
 202                                 m_ray                           =       guess;
 203                                 const btScalar  sqrl=   m_ray.length2();
 204                                 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
 205                                 m_simplices[0].p[0]     =       1;
 206                                 m_ray                           =       m_simplices[0].c[0]->w;
 207                                 sqdist                          =       sqrl;
 208                                 lastw[0]                        =
 209                                         lastw[1]                        =
 210                                         lastw[2]                        =
 211                                         lastw[3]                        =       m_ray;
 212                                 /* Loop                                         */
 213                                 do      {
 214                                         const U         next=1-m_current;
 215                                         sSimplex&       cs=m_simplices[m_current];
 216                                         sSimplex&       ns=m_simplices[next];
 217                                         /* Check zero                                                   */
 218                                         const btScalar  rl=m_ray.length();
 219                                         if(rl<GJK_MIN_DISTANCE)
 220                                         {/* Touching or inside                          */
 221                                                 m_status=eStatus::Inside;
 222                                                 break;
 223                                         }
 224                                         /* Append new vertice in -'v' direction */
 225                                         appendvertice(cs,-m_ray);
 226                                         const btVector3&        w=cs.c[cs.rank-1]->w;
 227                                         bool                            found=false;
 228                                         for(U i=0;i<4;++i)
 229                                         {
 230                                                 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
 231                                                 { found=true;break; }
 232                                         }
 233                                         if(found)
 234                                         {/* Return old simplex                          */
 235                                                 removevertice(m_simplices[m_current]);
 236                                                 break;
 237                                         }
 238                                         else
 239                                         {/* Update lastw                                        */
 240                                                 lastw[clastw=(clastw+1)&3]=w;
 241                                         }
 242                                         /* Check for termination                                */
 243                                         const btScalar  omega=btDot(m_ray,w)/rl;
 244                                         alpha=btMax(omega,alpha);
 245                                         if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
 246                                         {/* Return old simplex                          */
 247                                                 removevertice(m_simplices[m_current]);
 248                                                 break;
 249                                         }
 250                                         /* Reduce simplex                                               */
 251                                         btScalar        weights[4];
 252                                         U                       mask=0;
 253                                         switch(cs.rank)
 254                                         {
 255                                         case    2:      sqdist=projectorigin(   cs.c[0]->w,
 256                                                                         cs.c[1]->w,
 257                                                                         weights,mask);break;
 258                                         case    3:      sqdist=projectorigin(   cs.c[0]->w,
 259                                                                         cs.c[1]->w,
 260                                                                         cs.c[2]->w,
 261                                                                         weights,mask);break;
 262                                         case    4:      sqdist=projectorigin(   cs.c[0]->w,
 263                                                                         cs.c[1]->w,
 264                                                                         cs.c[2]->w,
 265                                                                         cs.c[3]->w,
 266                                                                         weights,mask);break;
 267                                         }
 268                                         if(sqdist>=0)
 269                                         {/* Valid       */
 270                                                 ns.rank         =       0;
 271                                                 m_ray           =       btVector3(0,0,0);
 272                                                 m_current       =       next;
 273                                                 for(U i=0,ni=cs.rank;i<ni;++i)
 274                                                 {
 275                                                         if(mask&(1<<i))
 276                                                         {
 277                                                                 ns.c[ns.rank]           =       cs.c[i];
 278                                                                 ns.p[ns.rank++]         =       weights[i];
 279                                                                 m_ray                           +=      cs.c[i]->w*weights[i];
 280                                                         }
 281                                                         else
 282                                                         {
 283                                                                 m_free[m_nfree++]       =       cs.c[i];
 284                                                         }
 285                                                 }
 286                                                 if(mask==15) m_status=eStatus::Inside;
 287                                         }
 288                                         else
 289                                         {/* Return old simplex                          */
 290                                                 removevertice(m_simplices[m_current]);
 291                                                 break;
 292                                         }
 293                                         m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
 294                                 } while(m_status==eStatus::Valid);
 295                                 m_simplex=&m_simplices[m_current];
 296                                 switch(m_status)
 297                                 {
 298                                 case    eStatus::Valid:         m_distance=m_ray.length();break;
 299                                 case    eStatus::Inside:        m_distance=0;break;
 300                                 default:
 301                                         {
 302                                         }
 303                                 }
 304                                 return(m_status);
 305                         }
 306                         bool                                    EncloseOrigin()
 307                         {
 308                                 switch(m_simplex->rank)
 309                                 {
 310                                 case    1:
 311                                         {
 312                                                 for(U i=0;i<3;++i)
 313                                                 {
 314                                                         btVector3               axis=btVector3(0,0,0);
 315                                                         axis[i]=1;
 316                                                         appendvertice(*m_simplex, axis);
 317                                                         if(EncloseOrigin())     return(true);
 318                                                         removevertice(*m_simplex);
 319                                                         appendvertice(*m_simplex,-axis);
 320                                                         if(EncloseOrigin())     return(true);
 321                                                         removevertice(*m_simplex);
 322                                                 }
 323                                         }
 324                                         break;
 325                                 case    2:
 326                                         {
 327                                                 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
 328                                                 for(U i=0;i<3;++i)
 329                                                 {
 330                                                         btVector3               axis=btVector3(0,0,0);
 331                                                         axis[i]=1;
 332                                                         const btVector3 p=btCross(d,axis);
 333                                                         if(p.length2()>0)
 334                                                         {
 335                                                                 appendvertice(*m_simplex, p);
 336                                                                 if(EncloseOrigin())     return(true);
 337                                                                 removevertice(*m_simplex);
 338                                                                 appendvertice(*m_simplex,-p);
 339                                                                 if(EncloseOrigin())     return(true);
 340                                                                 removevertice(*m_simplex);
 341                                                         }
 342                                                 }
 343                                         }
 344                                         break;
 345                                 case    3:
 346                                         {
 347                                                 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
 348                                                         m_simplex->c[2]->w-m_simplex->c[0]->w);
 349                                                 if(n.length2()>0)
 350                                                 {
 351                                                         appendvertice(*m_simplex,n);
 352                                                         if(EncloseOrigin())     return(true);
 353                                                         removevertice(*m_simplex);
 354                                                         appendvertice(*m_simplex,-n);
 355                                                         if(EncloseOrigin())     return(true);
 356                                                         removevertice(*m_simplex);
 357                                                 }
 358                                         }
 359                                         break;
 360                                 case    4:
 361                                         {
 362                                                 if(btFabs(det(  m_simplex->c[0]->w-m_simplex->c[3]->w,
 363                                                         m_simplex->c[1]->w-m_simplex->c[3]->w,
 364                                                         m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
 365                                                         return(true);
 366                                         }
 367                                         break;
 368                                 }
 369                                 return(false);
 370                         }
 371                         /* Internals    */
 372                         void                            getsupport(const btVector3& d,sSV& sv) const
 373                         {
 374                                 sv.d    =       d/d.length();
 375                                 sv.w    =       m_shape.Support(sv.d);
 376                         }
 377                         void                            removevertice(sSimplex& simplex)
 378                         {
 379                                 m_free[m_nfree++]=simplex.c[--simplex.rank];
 380                         }
 381                         void                            appendvertice(sSimplex& simplex,const btVector3& v)
 382                         {
 383                                 simplex.p[simplex.rank]=0;
 384                                 simplex.c[simplex.rank]=m_free[--m_nfree];
 385                                 getsupport(v,*simplex.c[simplex.rank++]);
 386                         }
 387                         static btScalar         det(const btVector3& a,const btVector3& b,const btVector3& c)
 388                         {
 389                                 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
 390                                         a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
 391                                         a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
 392                         }
 393                         static btScalar         projectorigin(  const btVector3& a,
 394                                 const btVector3& b,
 395                                 btScalar* w,U& m)
 396                         {
 397                                 const btVector3 d=b-a;
 398                                 const btScalar  l=d.length2();
 399                                 if(l>GJK_SIMPLEX2_EPS)
 400                                 {
 401                                         const btScalar  t(l>0?-btDot(a,d)/l:0);
 402                                         if(t>=1)                { w[0]=0;w[1]=1;m=2;return(b.length2()); }
 403                                         else if(t<=0)   { w[0]=1;w[1]=0;m=1;return(a.length2()); }
 404                                         else                    { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
 405                                 }
 406                                 return(-1);
 407                         }
 408                         static btScalar         projectorigin(  const btVector3& a,
 409                                 const btVector3& b,
 410                                 const btVector3& c,
 411                                 btScalar* w,U& m)
 412                         {
 413                                 static const U          imd3[]={1,2,0};
 414                                 const btVector3*        vt[]={&a,&b,&c};
 415                                 const btVector3         dl[]={a-b,b-c,c-a};
 416                                 const btVector3         n=btCross(dl[0],dl[1]);
 417                                 const btScalar          l=n.length2();
 418                                 if(l>GJK_SIMPLEX3_EPS)
 419                                 {
 420                                         btScalar        mindist=-1;
 421                                         btScalar        subw[2]={0.f,0.f};
 422                                         U                       subm(0);
 423                                         for(U i=0;i<3;++i)
 424                                         {
 425                                                 if(btDot(*vt[i],btCross(dl[i],n))>0)
 426                                                 {
 427                                                         const U                 j=imd3[i];
 428                                                         const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
 429                                                         if((mindist<0)||(subd<mindist))
 430                                                         {
 431                                                                 mindist         =       subd;
 432                                                                 m                       =       static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
 433                                                                 w[i]            =       subw[0];
 434                                                                 w[j]            =       subw[1];
 435                                                                 w[imd3[j]]      =       0;
 436                                                         }
 437                                                 }
 438                                         }
 439                                         if(mindist<0)
 440                                         {
 441                                                 const btScalar  d=btDot(a,n);
 442                                                 const btScalar  s=btSqrt(l);
 443                                                 const btVector3 p=n*(d/l);
 444                                                 mindist =       p.length2();
 445                                                 m               =       7;
 446                                                 w[0]    =       (btCross(dl[1],b-p)).length()/s;
 447                                                 w[1]    =       (btCross(dl[2],c-p)).length()/s;
 448                                                 w[2]    =       1-(w[0]+w[1]);
 449                                         }
 450                                         return(mindist);
 451                                 }
 452                                 return(-1);
 453                         }
 454                         static btScalar         projectorigin(  const btVector3& a,
 455                                 const btVector3& b,
 456                                 const btVector3& c,
 457                                 const btVector3& d,
 458                                 btScalar* w,U& m)
 459                         {
 460                                 static const U          imd3[]={1,2,0};
 461                                 const btVector3*        vt[]={&a,&b,&c,&d};
 462                                 const btVector3         dl[]={a-d,b-d,c-d};
 463                                 const btScalar          vl=det(dl[0],dl[1],dl[2]);
 464                                 const bool                      ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
 465                                 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
 466                                 {
 467                                         btScalar        mindist=-1;
 468                                         btScalar        subw[3]={0.f,0.f,0.f};
 469                                         U                       subm(0);
 470                                         for(U i=0;i<3;++i)
 471                                         {
 472                                                 const U                 j=imd3[i];
 473                                                 const btScalar  s=vl*btDot(d,btCross(dl[i],dl[j]));
 474                                                 if(s>0)
 475                                                 {
 476                                                         const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
 477                                                         if((mindist<0)||(subd<mindist))
 478                                                         {
 479                                                                 mindist         =       subd;
 480                                                                 m                       =       static_cast<U>((subm&1?1<<i:0)+
 481                                                                         (subm&2?1<<j:0)+
 482                                                                         (subm&4?8:0));
 483                                                                 w[i]            =       subw[0];
 484                                                                 w[j]            =       subw[1];
 485                                                                 w[imd3[j]]      =       0;
 486                                                                 w[3]            =       subw[2];
 487                                                         }
 488                                                 }
 489                                         }
 490                                         if(mindist<0)
 491                                         {
 492                                                 mindist =       0;
 493                                                 m               =       15;
 494                                                 w[0]    =       det(c,b,d)/vl;
 495                                                 w[1]    =       det(a,c,d)/vl;
 496                                                 w[2]    =       det(b,a,d)/vl;
 497                                                 w[3]    =       1-(w[0]+w[1]+w[2]);
 498                                         }
 499                                         return(mindist);
 500                                 }
 501                                 return(-1);
 502                         }
 503         };
 504
 505         // EPA
 506         struct  EPA
 507         {
 508                 /* Types                */
 509                 typedef GJK::sSV        sSV;
 510                 struct  sFace
 511                 {
 512                         btVector3       n;
 513                         btScalar        d;
 514                         btScalar        p;
 515                         sSV*            c[3];
 516                         sFace*          f[3];
 517                         sFace*          l[2];
 518                         U1                      e[3];
 519                         U1                      pass;
 520                 };
 521                 struct  sList
 522                 {
 523                         sFace*          root;
 524                         U                       count;
 525                         sList() : root(0),count(0)      {}
 526                 };
 527                 struct  sHorizon
 528                 {
 529                         sFace*          cf;
 530                         sFace*          ff;
 531                         U                       nf;
 532                         sHorizon() : cf(0),ff(0),nf(0)  {}
 533                 };
 534                 struct  eStatus { enum _ {
 535                         Valid,
 536                         Touching,
 537                         Degenerated,
 538                         NonConvex,
 539                         InvalidHull,
 540                         OutOfFaces,
 541                         OutOfVertices,
 542                         AccuraryReached,
 543                         FallBack,
 544                         Failed          };};
 545                         /* Fields               */
 546                         eStatus::_              m_status;
 547                         GJK::sSimplex   m_result;
 548                         btVector3               m_normal;
 549                         btScalar                m_depth;
 550                         sSV                             m_sv_store[EPA_MAX_VERTICES];
 551                         sFace                   m_fc_store[EPA_MAX_FACES];
 552                         U                               m_nextsv;
 553                         sList                   m_hull;
 554                         sList                   m_stock;
 555                         /* Methods              */
 556                         EPA()
 557                         {
 558                                 Initialize();
 559                         }
 560
 561
 562                         static inline void              bind(sFace* fa,U ea,sFace* fb,U eb)
 563                         {
 564                                 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
 565                                 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
 566                         }
 567                         static inline void              append(sList& list,sFace* face)
 568                         {
 569                                 face->l[0]      =       0;
 570                                 face->l[1]      =       list.root;
 571                                 if(list.root) list.root->l[0]=face;
 572                                 list.root       =       face;
 573                                 ++list.count;
 574                         }
 575                         static inline void              remove(sList& list,sFace* face)
 576                         {
 577                                 if(face->l[1]) face->l[1]->l[0]=face->l[0];
 578                                 if(face->l[0]) face->l[0]->l[1]=face->l[1];
 579                                 if(face==list.root) list.root=face->l[1];
 580                                 --list.count;
 581                         }
 582
 583
 584                         void                            Initialize()
 585                         {
 586                                 m_status        =       eStatus::Failed;
 587                                 m_normal        =       btVector3(0,0,0);
 588                                 m_depth         =       0;
 589                                 m_nextsv        =       0;
 590                                 for(U i=0;i<EPA_MAX_FACES;++i)
 591                                 {
 592                                         append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
 593                                 }
 594                         }
 595                         eStatus::_                      Evaluate(GJK& gjk,const btVector3& guess)
 596                         {
 597                                 GJK::sSimplex&  simplex=*gjk.m_simplex;
 598                                 if((simplex.rank>1)&&gjk.EncloseOrigin())
 599                                 {
 600
 601                                         /* Clean up                             */
 602                                         while(m_hull.root)
 603                                         {
 604                                                 sFace*  f = m_hull.root;
 605                                                 remove(m_hull,f);
 606                                                 append(m_stock,f);
 607                                         }
 608                                         m_status        =       eStatus::Valid;
 609                                         m_nextsv        =       0;
 610                                         /* Orient simplex               */
 611                                         if(gjk.det(     simplex.c[0]->w-simplex.c[3]->w,
 612                                                 simplex.c[1]->w-simplex.c[3]->w,
 613                                                 simplex.c[2]->w-simplex.c[3]->w)<0)
 614                                         {
 615                                                 btSwap(simplex.c[0],simplex.c[1]);
 616                                                 btSwap(simplex.p[0],simplex.p[1]);
 617                                         }
 618                                         /* Build initial hull   */
 619                                         sFace*  tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
 620                                                 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
 621                                                 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
 622                                                 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
 623                                         if(m_hull.count==4)
 624                                         {
 625                                                 sFace*          best=findbest();
 626                                                 sFace           outer=*best;
 627                                                 U                       pass=0;
 628                                                 U                       iterations=0;
 629                                                 bind(tetra[0],0,tetra[1],0);
 630                                                 bind(tetra[0],1,tetra[2],0);
 631                                                 bind(tetra[0],2,tetra[3],0);
 632                                                 bind(tetra[1],1,tetra[3],2);
 633                                                 bind(tetra[1],2,tetra[2],1);
 634                                                 bind(tetra[2],2,tetra[3],1);
 635                                                 m_status=eStatus::Valid;
 636                                                 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
 637                                                 {
 638                                                         if(m_nextsv<EPA_MAX_VERTICES)
 639                                                         {
 640                                                                 sHorizon                horizon;
 641                                                                 sSV*                    w=&m_sv_store[m_nextsv++];
 642                                                                 bool                    valid=true;
 643                                                                 best->pass      =       (U1)(++pass);
 644                                                                 gjk.getsupport(best->n,*w);
 645                                                                 const btScalar  wdist=btDot(best->n,w->w)-best->d;
 646                                                                 if(wdist>EPA_ACCURACY)
 647                                                                 {
 648                                                                         for(U j=0;(j<3)&&valid;++j)
 649                                                                         {
 650                                                                                 valid&=expand(  pass,w,
 651                                                                                         best->f[j],best->e[j],
 652                                                                                         horizon);
 653                                                                         }
 654                                                                         if(valid&&(horizon.nf>=3))
 655                                                                         {
 656                                                                                 bind(horizon.cf,1,horizon.ff,2);
 657                                                                                 remove(m_hull,best);
 658                                                                                 append(m_stock,best);
 659                                                                                 best=findbest();
 660                                                                                 if(best->p>=outer.p) outer=*best;
 661                                                                         } else { m_status=eStatus::InvalidHull;break; }
 662                                                                 } else { m_status=eStatus::AccuraryReached;break; }
 663                                                         } else { m_status=eStatus::OutOfVertices;break; }
 664                                                 }
 665                                                 const btVector3 projection=outer.n*outer.d;
 666                                                 m_normal        =       outer.n;
 667                                                 m_depth         =       outer.d;
 668                                                 m_result.rank   =       3;
 669                                                 m_result.c[0]   =       outer.c[0];
 670                                                 m_result.c[1]   =       outer.c[1];
 671                                                 m_result.c[2]   =       outer.c[2];
 672                                                 m_result.p[0]   =       btCross(        outer.c[1]->w-projection,
 673                                                         outer.c[2]->w-projection).length();
 674                                                 m_result.p[1]   =       btCross(        outer.c[2]->w-projection,
 675                                                         outer.c[0]->w-projection).length();
 676                                                 m_result.p[2]   =       btCross(        outer.c[0]->w-projection,
 677                                                         outer.c[1]->w-projection).length();
 678                                                 const btScalar  sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
 679                                                 m_result.p[0]   /=      sum;
 680                                                 m_result.p[1]   /=      sum;
 681                                                 m_result.p[2]   /=      sum;
 682                                                 return(m_status);
 683                                         }
 684                                 }
 685                                 /* Fallback             */
 686                                 m_status        =       eStatus::FallBack;
 687                                 m_normal        =       -guess;
 688                                 const btScalar  nl=m_normal.length();
 689                                 if(nl>0)
 690                                         m_normal        =       m_normal/nl;
 691                                 else
 692                                         m_normal        =       btVector3(1,0,0);
 693                                 m_depth =       0;
 694                                 m_result.rank=1;
 695                                 m_result.c[0]=simplex.c[0];
 696                                 m_result.p[0]=1;
 697                                 return(m_status);
 698                         }
 699                         sFace*                          newface(sSV* a,sSV* b,sSV* c,bool forced)
 700                         {
 701                                 if(m_stock.root)
 702                                 {
 703                                         sFace*  face=m_stock.root;
 704                                         remove(m_stock,face);
 705                                         append(m_hull,face);
 706                                         face->pass      =       0;
 707                                         face->c[0]      =       a;
 708                                         face->c[1]      =       b;
 709                                         face->c[2]      =       c;
 710                                         face->n         =       btCross(b->w-a->w,c->w-a->w);
 711                                         const btScalar  l=face->n.length();
 712                                         const bool              v=l>EPA_ACCURACY;
 713                                         face->p         =       btMin(btMin(
 714                                                 btDot(a->w,btCross(face->n,a->w-b->w)),
 715                                                 btDot(b->w,btCross(face->n,b->w-c->w))),
 716                                                 btDot(c->w,btCross(face->n,c->w-a->w))) /
 717                                                 (v?l:1);
 718                                         face->p         =       face->p>=-EPA_INSIDE_EPS?0:face->p;
 719                                         if(v)
 720                                         {
 721                                                 face->d         =       btDot(a->w,face->n)/l;
 722                                                 face->n         /=      l;
 723                                                 if(forced||(face->d>=-EPA_PLANE_EPS))
 724                                                 {
 725                                                         return(face);
 726                                                 } else m_status=eStatus::NonConvex;
 727                                         } else m_status=eStatus::Degenerated;
 728                                         remove(m_hull,face);
 729                                         append(m_stock,face);
 730                                         return(0);
 731                                 }
 732                                 m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
 733                                 return(0);
 734                         }
 735                         sFace*                          findbest()
 736                         {
 737                                 sFace*          minf=m_hull.root;
 738                                 btScalar        mind=minf->d*minf->d;
 739                                 btScalar        maxp=minf->p;
 740                                 for(sFace* f=minf->l[1];f;f=f->l[1])
 741                                 {
 742                                         const btScalar  sqd=f->d*f->d;
 743                                         if((f->p>=maxp)&&(sqd<mind))
 744                                         {
 745                                                 minf=f;
 746                                                 mind=sqd;
 747                                                 maxp=f->p;
 748                                         }
 749                                 }
 750                                 return(minf);
 751                         }
 752                         bool                            expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
 753                         {
 754                                 static const U  i1m3[]={1,2,0};
 755                                 static const U  i2m3[]={2,0,1};
 756                                 if(f->pass!=pass)
 757                                 {
 758                                         const U e1=i1m3[e];
 759                                         if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
 760                                         {
 761                                                 sFace*  nf=newface(f->c[e1],f->c[e],w,false);
 762                                                 if(nf)
 763                                                 {
 764                                                         bind(nf,0,f,e);
 765                                                         if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
 766                                                         horizon.cf=nf;
 767                                                         ++horizon.nf;
 768                                                         return(true);
 769                                                 }
 770                                         }
 771                                         else
 772                                         {
 773                                                 const U e2=i2m3[e];
 774                                                 f->pass         =       (U1)pass;
 775                                                 if(     expand(pass,w,f->f[e1],f->e[e1],horizon)&&
 776                                                         expand(pass,w,f->f[e2],f->e[e2],horizon))
 777                                                 {
 778                                                         remove(m_hull,f);
 779                                                         append(m_stock,f);
 780                                                         return(true);
 781                                                 }
 782                                         }
 783                                 }
 784                                 return(false);
 785                         }
 786
 787         };
 788
 789         //
 790         static void     Initialize(     const btConvexShape* shape0,const btTransform& wtrs0,
 791                 const btConvexShape* shape1,const btTransform& wtrs1,
 792                 btGjkEpaSolver2::sResults& results,
 793                 tShape& shape,
 794                 bool withmargins)
 795         {
 796                 /* Results              */
 797                 results.witnesses[0]    =
 798                         results.witnesses[1]    =       btVector3(0,0,0);
 799                 results.status                  =       btGjkEpaSolver2::sResults::Separated;
 800                 /* Shape                */
 801                 shape.m_shapes[0]               =       shape0;
 802                 shape.m_shapes[1]               =       shape1;
 803                 shape.m_toshape1                =       wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
 804                 shape.m_toshape0                =       wtrs0.inverseTimes(wtrs1);
 805                 shape.EnableMargin(withmargins);
 806         }
 807
 808 }
 809
 810 //
 811 // Api
 812 //
 813
 814 using namespace gjkepa2_impl;
 815
 816 //
 817 int                     btGjkEpaSolver2::StackSizeRequirement()
 818 {
 819         return(sizeof(GJK)+sizeof(EPA));
 820 }
 821
 822 //
 823 bool            btGjkEpaSolver2::Distance(      const btConvexShape*    shape0,
 824                                                                           const btTransform&            wtrs0,
 825                                                                           const btConvexShape*  shape1,
 826                                                                           const btTransform&            wtrs1,
 827                                                                           const btVector3&              guess,
 828                                                                           sResults&                             results)
 829 {
 830         tShape                  shape;
 831         Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
 832         GJK                             gjk;
 833         GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
 834         if(gjk_status==GJK::eStatus::Valid)
 835         {
 836                 btVector3       w0=btVector3(0,0,0);
 837                 btVector3       w1=btVector3(0,0,0);
 838                 for(U i=0;i<gjk.m_simplex->rank;++i)
 839                 {
 840                         const btScalar  p=gjk.m_simplex->p[i];
 841                         w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
 842                         w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
 843                 }
 844                 results.witnesses[0]    =       wtrs0*w0;
 845                 results.witnesses[1]    =       wtrs0*w1;
 846                 results.normal                  =       w0-w1;
 847                 results.distance                =       results.normal.length();
 848                 results.normal                  /=      results.distance>GJK_MIN_DISTANCE?results.distance:1;
 849                 return(true);
 850         }
 851         else
 852         {
 853                 results.status  =       gjk_status==GJK::eStatus::Inside?
 854                         sResults::Penetrating   :
 855                 sResults::GJK_Failed    ;
 856                 return(false);
 857         }
 858 }
 859
 860 //
 861 bool    btGjkEpaSolver2::Penetration(   const btConvexShape*    shape0,
 862                                                                          const btTransform&             wtrs0,
 863                                                                          const btConvexShape*   shape1,
 864                                                                          const btTransform&             wtrs1,
 865                                                                          const btVector3&               guess,
 866                                                                          sResults&                              results,
 867                                                                          bool                                   usemargins)
 868 {
 869         tShape                  shape;
 870         Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
 871         GJK                             gjk;
 872         GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
 873         switch(gjk_status)
 874         {
 875         case    GJK::eStatus::Inside:
 876                 {
 877                         EPA                             epa;
 878                         EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
 879                         if(epa_status!=EPA::eStatus::Failed)
 880                         {
 881                                 btVector3       w0=btVector3(0,0,0);
 882                                 for(U i=0;i<epa.m_result.rank;++i)
 883                                 {
 884                                         w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
 885                                 }
 886                                 results.status                  =       sResults::Penetrating;
 887                                 results.witnesses[0]    =       wtrs0*w0;
 888                                 results.witnesses[1]    =       wtrs0*(w0-epa.m_normal*epa.m_depth);
 889                                 results.normal                  =       -epa.m_normal;
 890                                 results.distance                =       -epa.m_depth;
 891                                 return(true);
 892                         } else results.status=sResults::EPA_Failed;
 893                 }
 894                 break;
 895         case    GJK::eStatus::Failed:
 896                 results.status=sResults::GJK_Failed;
 897                 break;
 898                 default:
 899                                         {
 900                                         }
 901         }
 902         return(false);
 903 }
 904
 905 #ifndef __SPU__
 906 //
 907 btScalar        btGjkEpaSolver2::SignedDistance(const btVector3& position,
 908                                                                                         btScalar margin,
 909                                                                                         const btConvexShape* shape0,
 910                                                                                         const btTransform& wtrs0,
 911                                                                                         sResults& results)
 912 {
 913         tShape                  shape;
 914         btSphereShape   shape1(margin);
 915         btTransform             wtrs1(btQuaternion(0,0,0,1),position);
 916         Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
 917         GJK                             gjk;
 918         GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
 919         if(gjk_status==GJK::eStatus::Valid)
 920         {
 921                 btVector3       w0=btVector3(0,0,0);
 922                 btVector3       w1=btVector3(0,0,0);
 923                 for(U i=0;i<gjk.m_simplex->rank;++i)
 924                 {
 925                         const btScalar  p=gjk.m_simplex->p[i];
 926                         w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
 927                         w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
 928                 }
 929                 results.witnesses[0]    =       wtrs0*w0;
 930                 results.witnesses[1]    =       wtrs0*w1;
 931                 const btVector3 delta=  results.witnesses[1]-
 932                         results.witnesses[0];
 933                 const btScalar  margin= shape0->getMarginNonVirtual()+
 934                         shape1.getMarginNonVirtual();
 935                 const btScalar  length= delta.length();
 936                 results.normal                  =       delta/length;
 937                 results.witnesses[0]    +=      results.normal*margin;
 938                 return(length-margin);
 939         }
 940         else
 941         {
 942                 if(gjk_status==GJK::eStatus::Inside)
 943                 {
 944                         if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
 945                         {
 946                                 const btVector3 delta=  results.witnesses[0]-
 947                                         results.witnesses[1];
 948                                 const btScalar  length= delta.length();
 949                                 if (length >= SIMD_EPSILON)
 950                                         results.normal  =       delta/length;
 951                                 return(-length);
 952                         }
 953                 }
 954         }
 955         return(SIMD_INFINITY);
 956 }
 957
 958 //
 959 bool    btGjkEpaSolver2::SignedDistance(const btConvexShape*    shape0,
 960                                                                                 const btTransform&              wtrs0,
 961                                                                                 const btConvexShape*    shape1,
 962                                                                                 const btTransform&              wtrs1,
 963                                                                                 const btVector3&                guess,
 964                                                                                 sResults&                               results)
 965 {
 966         if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
 967                 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
 968         else
 969                 return(true);
 970 }
 971 #endif //__SPU__
 972
 973 /* Symbols cleanup              */
 974
 975 #undef GJK_MAX_ITERATIONS
 976 #undef GJK_ACCURARY
 977 #undef GJK_MIN_DISTANCE
 978 #undef GJK_DUPLICATED_EPS
 979 #undef GJK_SIMPLEX2_EPS
 980 #undef GJK_SIMPLEX3_EPS
 981 #undef GJK_SIMPLEX4_EPS
 982
 983 #undef EPA_MAX_VERTICES
 984 #undef EPA_MAX_FACES
 985 #undef EPA_MAX_ITERATIONS
 986 #undef EPA_ACCURACY
 987 #undef EPA_FALLBACK
 988 #undef EPA_PLANE_EPS
 989 #undef EPA_INSIDE_EPS