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