dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

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