dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h

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