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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 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 use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btGjkPairDetector.h"
  17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  18 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
  19 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
  20
  21 #if defined(DEBUG) || defined(_DEBUG)
  22 //#define TEST_NON_VIRTUAL 1
  23 #include <stdio.h>  //for debug printf
  24 #ifdef __SPU__
  25 #include <spu_printf.h>
  26 #define printf spu_printf
  27 #endif  //__SPU__
  28 #endif
  29
  30 //must be above the machine epsilon
  31 #ifdef BT_USE_DOUBLE_PRECISION
  32 #define REL_ERROR2 btScalar(1.0e-12)
  33 btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
  34 #else
  35 #define REL_ERROR2 btScalar(1.0e-6)
  36 btScalar gGjkEpaPenetrationTolerance = 0.001;
  37 #endif
  38
  39
  40 btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
  41         : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
  42           m_penetrationDepthSolver(penetrationDepthSolver),
  43           m_simplexSolver(simplexSolver),
  44           m_minkowskiA(objectA),
  45           m_minkowskiB(objectB),
  46           m_shapeTypeA(objectA->getShapeType()),
  47           m_shapeTypeB(objectB->getShapeType()),
  48           m_marginA(objectA->getMargin()),
  49           m_marginB(objectB->getMargin()),
  50           m_ignoreMargin(false),
  51           m_lastUsedMethod(-1),
  52           m_catchDegeneracies(1),
  53           m_fixContactNormalDirection(1)
  54 {
  55 }
  56 btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
  57         : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
  58           m_penetrationDepthSolver(penetrationDepthSolver),
  59           m_simplexSolver(simplexSolver),
  60           m_minkowskiA(objectA),
  61           m_minkowskiB(objectB),
  62           m_shapeTypeA(shapeTypeA),
  63           m_shapeTypeB(shapeTypeB),
  64           m_marginA(marginA),
  65           m_marginB(marginB),
  66           m_ignoreMargin(false),
  67           m_lastUsedMethod(-1),
  68           m_catchDegeneracies(1),
  69           m_fixContactNormalDirection(1)
  70 {
  71 }
  72
  73 void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults)
  74 {
  75         (void)swapResults;
  76
  77         getClosestPointsNonVirtual(input, output, debugDraw);
  78 }
  79
  80 static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
  81 {
  82         btVector3 separatingAxisInA = (dir)*localTransA.getBasis();
  83         btVector3 separatingAxisInB = (-dir) * localTransB.getBasis();
  84
  85         btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
  86         btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
  87
  88         btVector3 pInA = pInANoMargin;
  89         btVector3 qInB = qInBNoMargin;
  90
  91         supAworld = localTransA(pInA);
  92         supBworld = localTransB(qInB);
  93
  94         if (check2d)
  95         {
  96                 supAworld[2] = 0.f;
  97                 supBworld[2] = 0.f;
  98         }
  99
 100         aMinb = supAworld - supBworld;
 101 }
 102
 103 struct btSupportVector
 104 {
 105         btVector3 v;   //!< Support point in minkowski sum
 106         btVector3 v1;  //!< Support point in obj1
 107         btVector3 v2;  //!< Support point in obj2
 108 };
 109
 110 struct btSimplex
 111 {
 112         btSupportVector ps[4];
 113         int last;  //!< index of last added point
 114 };
 115
 116 static btVector3 ccd_vec3_origin(0, 0, 0);
 117
 118 inline void btSimplexInit(btSimplex *s)
 119 {
 120         s->last = -1;
 121 }
 122
 123 inline int btSimplexSize(const btSimplex *s)
 124 {
 125         return s->last + 1;
 126 }
 127
 128 inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx)
 129 {
 130         // here is no check on boundaries
 131         return &s->ps[idx];
 132 }
 133 inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
 134 {
 135         *d = *s;
 136 }
 137
 138 inline void btVec3Copy(btVector3 *v, const btVector3 *w)
 139 {
 140         *v = *w;
 141 }
 142
 143 inline void ccdVec3Add(btVector3 *v, const btVector3 *w)
 144 {
 145         v->m_floats[0] += w->m_floats[0];
 146         v->m_floats[1] += w->m_floats[1];
 147         v->m_floats[2] += w->m_floats[2];
 148 }
 149
 150 inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
 151 {
 152         *v -= *w;
 153 }
 154 inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
 155 {
 156         *d = (*v) - (*w);
 157 }
 158 inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
 159 {
 160         btScalar dot;
 161         dot = a->dot(*b);
 162
 163         return dot;
 164 }
 165
 166 inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
 167 {
 168         btVector3 ab;
 169         btVec3Sub2(&ab, a, b);
 170         return btVec3Dot(&ab, &ab);
 171 }
 172
 173 inline void btVec3Scale(btVector3 *d, btScalar k)
 174 {
 175         d->m_floats[0] *= k;
 176         d->m_floats[1] *= k;
 177         d->m_floats[2] *= k;
 178 }
 179
 180 inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
 181 {
 182         d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]);
 183         d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]);
 184         d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]);
 185 }
 186
 187 inline void btTripleCross(const btVector3 *a, const btVector3 *b,
 188                                                   const btVector3 *c, btVector3 *d)
 189 {
 190         btVector3 e;
 191         btVec3Cross(&e, a, b);
 192         btVec3Cross(d, &e, c);
 193 }
 194
 195 inline int ccdEq(btScalar _a, btScalar _b)
 196 {
 197         btScalar ab;
 198         btScalar a, b;
 199
 200         ab = btFabs(_a - _b);
 201         if (btFabs(ab) < SIMD_EPSILON)
 202                 return 1;
 203
 204         a = btFabs(_a);
 205         b = btFabs(_b);
 206         if (b > a)
 207         {
 208                 return ab < SIMD_EPSILON * b;
 209         }
 210         else
 211         {
 212                 return ab < SIMD_EPSILON * a;
 213         }
 214 }
 215
 216 btScalar ccdVec3X(const btVector3 *v)
 217 {
 218         return v->x();
 219 }
 220
 221 btScalar ccdVec3Y(const btVector3 *v)
 222 {
 223         return v->y();
 224 }
 225
 226 btScalar ccdVec3Z(const btVector3 *v)
 227 {
 228         return v->z();
 229 }
 230 inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
 231 {
 232         return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
 233 }
 234
 235 inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
 236 {
 237         // here is no check on boundaries in sake of speed
 238         ++s->last;
 239         btSupportCopy(s->ps + s->last, v);
 240 }
 241
 242 inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
 243 {
 244         btSupportCopy(s->ps + pos, a);
 245 }
 246
 247 inline void btSimplexSetSize(btSimplex *s, int size)
 248 {
 249         s->last = size - 1;
 250 }
 251
 252 inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
 253 {
 254         return btSimplexPoint(s, s->last);
 255 }
 256
 257 inline int ccdSign(btScalar val)
 258 {
 259         if (btFuzzyZero(val))
 260         {
 261                 return 0;
 262         }
 263         else if (val < btScalar(0))
 264         {
 265                 return -1;
 266         }
 267         return 1;
 268 }
 269
 270 inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
 271                                                                                 const btVector3 *x0,
 272                                                                                 const btVector3 *b,
 273                                                                                 btVector3 *witness)
 274 {
 275         // The computation comes from solving equation of segment:
 276         //      S(t) = x0 + t.d
 277         //          where - x0 is initial point of segment
 278         //                - d is direction of segment from x0 (|d| > 0)
 279         //                - t belongs to <0, 1> interval
 280         //
 281         // Than, distance from a segment to some point P can be expressed:
 282         //      D(t) = |x0 + t.d - P|^2
 283         //          which is distance from any point on segment. Minimization
 284         //          of this function brings distance from P to segment.
 285         // Minimization of D(t) leads to simple quadratic equation that's
 286         // solving is straightforward.
 287         //
 288         // Bonus of this method is witness point for free.
 289
 290         btScalar dist, t;
 291         btVector3 d, a;
 292
 293         // direction of segment
 294         btVec3Sub2(&d, b, x0);
 295
 296         // precompute vector from P to x0
 297         btVec3Sub2(&a, x0, P);
 298
 299         t = -btScalar(1.) * btVec3Dot(&a, &d);
 300         t /= btVec3Dot(&d, &d);
 301
 302         if (t < btScalar(0) || btFuzzyZero(t))
 303         {
 304                 dist = ccdVec3Dist2(x0, P);
 305                 if (witness)
 306                         btVec3Copy(witness, x0);
 307         }
 308         else if (t > btScalar(1) || ccdEq(t, btScalar(1)))
 309         {
 310                 dist = ccdVec3Dist2(b, P);
 311                 if (witness)
 312                         btVec3Copy(witness, b);
 313         }
 314         else
 315         {
 316                 if (witness)
 317                 {
 318                         btVec3Copy(witness, &d);
 319                         btVec3Scale(witness, t);
 320                         ccdVec3Add(witness, x0);
 321                         dist = ccdVec3Dist2(witness, P);
 322                 }
 323                 else
 324                 {
 325                         // recycling variables
 326                         btVec3Scale(&d, t);
 327                         ccdVec3Add(&d, &a);
 328                         dist = btVec3Dot(&d, &d);
 329                 }
 330         }
 331
 332         return dist;
 333 }
 334
 335 btScalar btVec3PointTriDist2(const btVector3 *P,
 336                                                          const btVector3 *x0, const btVector3 *B,
 337                                                          const btVector3 *C,
 338                                                          btVector3 *witness)
 339 {
 340         // Computation comes from analytic expression for triangle (x0, B, C)
 341         //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
 342         // Then equation for distance is:
 343         //      D(s, t) = | T(s, t) - P |^2
 344         // This leads to minimization of quadratic function of two variables.
 345         // The solution from is taken only if s is between 0 and 1, t is
 346         // between 0 and 1 and t + s < 1, otherwise distance from segment is
 347         // computed.
 348
 349         btVector3 d1, d2, a;
 350         double u, v, w, p, q, r;
 351         double s, t, dist, dist2;
 352         btVector3 witness2;
 353
 354         btVec3Sub2(&d1, B, x0);
 355         btVec3Sub2(&d2, C, x0);
 356         btVec3Sub2(&a, x0, P);
 357
 358         u = btVec3Dot(&a, &a);
 359         v = btVec3Dot(&d1, &d1);
 360         w = btVec3Dot(&d2, &d2);
 361         p = btVec3Dot(&a, &d1);
 362         q = btVec3Dot(&a, &d2);
 363         r = btVec3Dot(&d1, &d2);
 364
 365         s = (q * r - w * p) / (w * v - r * r);
 366         t = (-s * r - q) / w;
 367
 368         if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1)))
 369         {
 370                 if (witness)
 371                 {
 372                         btVec3Scale(&d1, s);
 373                         btVec3Scale(&d2, t);
 374                         btVec3Copy(witness, x0);
 375                         ccdVec3Add(witness, &d1);
 376                         ccdVec3Add(witness, &d2);
 377
 378                         dist = ccdVec3Dist2(witness, P);
 379                 }
 380                 else
 381                 {
 382                         dist = s * s * v;
 383                         dist += t * t * w;
 384                         dist += btScalar(2.) * s * t * r;
 385                         dist += btScalar(2.) * s * p;
 386                         dist += btScalar(2.) * t * q;
 387                         dist += u;
 388                 }
 389         }
 390         else
 391         {
 392                 dist = btVec3PointSegmentDist2(P, x0, B, witness);
 393
 394                 dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
 395                 if (dist2 < dist)
 396                 {
 397                         dist = dist2;
 398                         if (witness)
 399                                 btVec3Copy(witness, &witness2);
 400                 }
 401
 402                 dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
 403                 if (dist2 < dist)
 404                 {
 405                         dist = dist2;
 406                         if (witness)
 407                                 btVec3Copy(witness, &witness2);
 408                 }
 409         }
 410
 411         return dist;
 412 }
 413
 414 static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
 415 {
 416         const btSupportVector *A, *B;
 417         btVector3 AB, AO, tmp;
 418         btScalar dot;
 419
 420         // get last added as A
 421         A = ccdSimplexLast(simplex);
 422         // get the other point
 423         B = btSimplexPoint(simplex, 0);
 424         // compute AB oriented segment
 425         btVec3Sub2(&AB, &B->v, &A->v);
 426         // compute AO vector
 427         btVec3Copy(&AO, &A->v);
 428         btVec3Scale(&AO, -btScalar(1));
 429
 430         // dot product AB . AO
 431         dot = btVec3Dot(&AB, &AO);
 432
 433         // check if origin doesn't lie on AB segment
 434         btVec3Cross(&tmp, &AB, &AO);
 435         if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0))
 436         {
 437                 return 1;
 438         }
 439
 440         // check if origin is in area where AB segment is
 441         if (btFuzzyZero(dot) || dot < btScalar(0))
 442         {
 443                 // origin is in outside are of A
 444                 btSimplexSet(simplex, 0, A);
 445                 btSimplexSetSize(simplex, 1);
 446                 btVec3Copy(dir, &AO);
 447         }
 448         else
 449         {
 450                 // origin is in area where AB segment is
 451
 452                 // keep simplex untouched and set direction to
 453                 // AB x AO x AB
 454                 btTripleCross(&AB, &AO, &AB, dir);
 455         }
 456
 457         return 0;
 458 }
 459
 460 static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
 461 {
 462         const btSupportVector *A, *B, *C;
 463         btVector3 AO, AB, AC, ABC, tmp;
 464         btScalar dot, dist;
 465
 466         // get last added as A
 467         A = ccdSimplexLast(simplex);
 468         // get the other points
 469         B = btSimplexPoint(simplex, 1);
 470         C = btSimplexPoint(simplex, 0);
 471
 472         // check touching contact
 473         dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
 474         if (btFuzzyZero(dist))
 475         {
 476                 return 1;
 477         }
 478
 479         // check if triangle is really triangle (has area > 0)
 480         // if not simplex can't be expanded and thus no itersection is found
 481         if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v))
 482         {
 483                 return -1;
 484         }
 485
 486         // compute AO vector
 487         btVec3Copy(&AO, &A->v);
 488         btVec3Scale(&AO, -btScalar(1));
 489
 490         // compute AB and AC segments and ABC vector (perpendircular to triangle)
 491         btVec3Sub2(&AB, &B->v, &A->v);
 492         btVec3Sub2(&AC, &C->v, &A->v);
 493         btVec3Cross(&ABC, &AB, &AC);
 494
 495         btVec3Cross(&tmp, &ABC, &AC);
 496         dot = btVec3Dot(&tmp, &AO);
 497         if (btFuzzyZero(dot) || dot > btScalar(0))
 498         {
 499                 dot = btVec3Dot(&AC, &AO);
 500                 if (btFuzzyZero(dot) || dot > btScalar(0))
 501                 {
 502                         // C is already in place
 503                         btSimplexSet(simplex, 1, A);
 504                         btSimplexSetSize(simplex, 2);
 505                         btTripleCross(&AC, &AO, &AC, dir);
 506                 }
 507                 else
 508                 {
 509                         dot = btVec3Dot(&AB, &AO);
 510                         if (btFuzzyZero(dot) || dot > btScalar(0))
 511                         {
 512                                 btSimplexSet(simplex, 0, B);
 513                                 btSimplexSet(simplex, 1, A);
 514                                 btSimplexSetSize(simplex, 2);
 515                                 btTripleCross(&AB, &AO, &AB, dir);
 516                         }
 517                         else
 518                         {
 519                                 btSimplexSet(simplex, 0, A);
 520                                 btSimplexSetSize(simplex, 1);
 521                                 btVec3Copy(dir, &AO);
 522                         }
 523                 }
 524         }
 525         else
 526         {
 527                 btVec3Cross(&tmp, &AB, &ABC);
 528                 dot = btVec3Dot(&tmp, &AO);
 529                 if (btFuzzyZero(dot) || dot > btScalar(0))
 530                 {
 531                         dot = btVec3Dot(&AB, &AO);
 532                         if (btFuzzyZero(dot) || dot > btScalar(0))
 533                         {
 534                                 btSimplexSet(simplex, 0, B);
 535                                 btSimplexSet(simplex, 1, A);
 536                                 btSimplexSetSize(simplex, 2);
 537                                 btTripleCross(&AB, &AO, &AB, dir);
 538                         }
 539                         else
 540                         {
 541                                 btSimplexSet(simplex, 0, A);
 542                                 btSimplexSetSize(simplex, 1);
 543                                 btVec3Copy(dir, &AO);
 544                         }
 545                 }
 546                 else
 547                 {
 548                         dot = btVec3Dot(&ABC, &AO);
 549                         if (btFuzzyZero(dot) || dot > btScalar(0))
 550                         {
 551                                 btVec3Copy(dir, &ABC);
 552                         }
 553                         else
 554                         {
 555                                 btSupportVector tmp;
 556                                 btSupportCopy(&tmp, C);
 557                                 btSimplexSet(simplex, 0, B);
 558                                 btSimplexSet(simplex, 1, &tmp);
 559
 560                                 btVec3Copy(dir, &ABC);
 561                                 btVec3Scale(dir, -btScalar(1));
 562                         }
 563                 }
 564         }
 565
 566         return 0;
 567 }
 568
 569 static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
 570 {
 571         const btSupportVector *A, *B, *C, *D;
 572         btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
 573         int B_on_ACD, C_on_ADB, D_on_ABC;
 574         int AB_O, AC_O, AD_O;
 575         btScalar dist;
 576
 577         // get last added as A
 578         A = ccdSimplexLast(simplex);
 579         // get the other points
 580         B = btSimplexPoint(simplex, 2);
 581         C = btSimplexPoint(simplex, 1);
 582         D = btSimplexPoint(simplex, 0);
 583
 584         // check if tetrahedron is really tetrahedron (has volume > 0)
 585         // if it is not simplex can't be expanded and thus no intersection is
 586         // found
 587         dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
 588         if (btFuzzyZero(dist))
 589         {
 590                 return -1;
 591         }
 592
 593         // check if origin lies on some of tetrahedron's face - if so objects
 594         // intersect
 595         dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
 596         if (btFuzzyZero(dist))
 597                 return 1;
 598         dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0);
 599         if (btFuzzyZero(dist))
 600                 return 1;
 601         dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0);
 602         if (btFuzzyZero(dist))
 603                 return 1;
 604         dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0);
 605         if (btFuzzyZero(dist))
 606                 return 1;
 607
 608         // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
 609         btVec3Copy(&AO, &A->v);
 610         btVec3Scale(&AO, -btScalar(1));
 611         btVec3Sub2(&AB, &B->v, &A->v);
 612         btVec3Sub2(&AC, &C->v, &A->v);
 613         btVec3Sub2(&AD, &D->v, &A->v);
 614         btVec3Cross(&ABC, &AB, &AC);
 615         btVec3Cross(&ACD, &AC, &AD);
 616         btVec3Cross(&ADB, &AD, &AB);
 617
 618         // side (positive or negative) of B, C, D relative to planes ACD, ADB
 619         // and ABC respectively
 620         B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
 621         C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
 622         D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
 623
 624         // whether origin is on same side of ACD, ADB, ABC as B, C, D
 625         // respectively
 626         AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;
 627         AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
 628         AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
 629
 630         if (AB_O && AC_O && AD_O)
 631         {
 632                 // origin is in tetrahedron
 633                 return 1;
 634                 // rearrange simplex to triangle and call btDoSimplex3()
 635         }
 636         else if (!AB_O)
 637         {
 638                 // B is farthest from the origin among all of the tetrahedron's
 639                 // points, so remove it from the list and go on with the triangle
 640                 // case
 641
 642                 // D and C are in place
 643                 btSimplexSet(simplex, 2, A);
 644                 btSimplexSetSize(simplex, 3);
 645         }
 646         else if (!AC_O)
 647         {
 648                 // C is farthest
 649                 btSimplexSet(simplex, 1, D);
 650                 btSimplexSet(simplex, 0, B);
 651                 btSimplexSet(simplex, 2, A);
 652                 btSimplexSetSize(simplex, 3);
 653         }
 654         else
 655         {  // (!AD_O)
 656                 btSimplexSet(simplex, 0, C);
 657                 btSimplexSet(simplex, 1, B);
 658                 btSimplexSet(simplex, 2, A);
 659                 btSimplexSetSize(simplex, 3);
 660         }
 661
 662         return btDoSimplex3(simplex, dir);
 663 }
 664
 665 static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
 666 {
 667         if (btSimplexSize(simplex) == 2)
 668         {
 669                 // simplex contains segment only one segment
 670                 return btDoSimplex2(simplex, dir);
 671         }
 672         else if (btSimplexSize(simplex) == 3)
 673         {
 674                 // simplex contains triangle
 675                 return btDoSimplex3(simplex, dir);
 676         }
 677         else
 678         {  // btSimplexSize(simplex) == 4
 679                 // tetrahedron - this is the only shape which can encapsule origin
 680                 // so btDoSimplex4() also contains test on it
 681                 return btDoSimplex4(simplex, dir);
 682         }
 683 }
 684
 685 #ifdef __SPU__
 686 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
 687 #else
 688 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
 689 #endif
 690 {
 691         m_cachedSeparatingDistance = 0.f;
 692
 693         btScalar distance = btScalar(0.);
 694         btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
 695
 696         btVector3 pointOnA, pointOnB;
 697         btTransform localTransA = input.m_transformA;
 698         btTransform localTransB = input.m_transformB;
 699         btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
 700         localTransA.getOrigin() -= positionOffset;
 701         localTransB.getOrigin() -= positionOffset;
 702
 703         bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
 704
 705         btScalar marginA = m_marginA;
 706         btScalar marginB = m_marginB;
 707
 708
 709         //for CCD we don't use margins
 710         if (m_ignoreMargin)
 711         {
 712                 marginA = btScalar(0.);
 713                 marginB = btScalar(0.);
 714         }
 715
 716         m_curIter = 0;
 717         int gGjkMaxIter = 1000;  //this is to catch invalid input, perhaps check for #NaN?
 718         m_cachedSeparatingAxis.setValue(0, 1, 0);
 719
 720         bool isValid = false;
 721         bool checkSimplex = false;
 722         bool checkPenetration = true;
 723         m_degenerateSimplex = 0;
 724
 725         m_lastUsedMethod = -1;
 726         int status = -2;
 727         btVector3 orgNormalInB(0, 0, 0);
 728         btScalar margin = marginA + marginB;
 729
 730         //we add a separate implementation to check if the convex shapes intersect
 731         //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
 732         //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
 733         //and remove this temporary code from libCCD
 734         //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703
 735         //note, for large differences in shapes, use double precision build!
 736         {
 737                 btScalar squaredDistance = BT_LARGE_FLOAT;
 738                 btScalar delta = btScalar(0.);
 739
 740                 btSimplex simplex1;
 741                 btSimplex *simplex = &simplex1;
 742                 btSimplexInit(simplex);
 743
 744                 btVector3 dir(1, 0, 0);
 745
 746                 {
 747                         btVector3 lastSupV;
 748                         btVector3 supAworld;
 749                         btVector3 supBworld;
 750                         btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 751
 752                         btSupportVector last;
 753                         last.v = lastSupV;
 754                         last.v1 = supAworld;
 755                         last.v2 = supBworld;
 756
 757                         btSimplexAdd(simplex, &last);
 758
 759                         dir = -lastSupV;
 760
 761                         // start iterations
 762                         for (int iterations = 0; iterations < gGjkMaxIter; iterations++)
 763                         {
 764                                 // obtain support point
 765                                 btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 766
 767                                 // check if farthest point in Minkowski difference in direction dir
 768                                 // isn't somewhere before origin (the test on negative dot product)
 769                                 // - because if it is, objects are not intersecting at all.
 770                                 btScalar delta = lastSupV.dot(dir);
 771                                 if (delta < 0)
 772                                 {
 773                                         //no intersection, besides margin
 774                                         status = -1;
 775                                         break;
 776                                 }
 777
 778                                 // add last support vector to simplex
 779                                 last.v = lastSupV;
 780                                 last.v1 = supAworld;
 781                                 last.v2 = supBworld;
 782
 783                                 btSimplexAdd(simplex, &last);
 784
 785                                 // if btDoSimplex returns 1 if objects intersect, -1 if objects don't
 786                                 // intersect and 0 if algorithm should continue
 787
 788                                 btVector3 newDir;
 789                                 int do_simplex_res = btDoSimplex(simplex, &dir);
 790
 791                                 if (do_simplex_res == 1)
 792                                 {
 793                                         status = 0;  // intersection found
 794                                         break;
 795                                 }
 796                                 else if (do_simplex_res == -1)
 797                                 {
 798                                         // intersection not found
 799                                         status = -1;
 800                                         break;
 801                                 }
 802
 803                                 if (btFuzzyZero(btVec3Dot(&dir, &dir)))
 804                                 {
 805                                         // intersection not found
 806                                         status = -1;
 807                                 }
 808
 809                                 if (dir.length2() < SIMD_EPSILON)
 810                                 {
 811                                         //no intersection, besides margin
 812                                         status = -1;
 813                                         break;
 814                                 }
 815
 816                                 if (dir.fuzzyZero())
 817                                 {
 818                                         // intersection not found
 819                                         status = -1;
 820                                         break;
 821                                 }
 822                         }
 823                 }
 824
 825                 m_simplexSolver->reset();
 826                 if (status == 0)
 827                 {
 828                         //status = 0;
 829                         //printf("Intersect!\n");
 830                 }
 831
 832                 if (status == -1)
 833                 {
 834                         //printf("not intersect\n");
 835                 }
 836                 //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
 837                 if (1)
 838                 {
 839                         for (;;)
 840                         //while (true)
 841                         {
 842                                 btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
 843                                 btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
 844
 845                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
 846                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 847
 848                                 btVector3 pWorld = localTransA(pInA);
 849                                 btVector3 qWorld = localTransB(qInB);
 850
 851                                 if (check2d)
 852                                 {
 853                                         pWorld[2] = 0.f;
 854                                         qWorld[2] = 0.f;
 855                                 }
 856
 857                                 btVector3 w = pWorld - qWorld;
 858                                 delta = m_cachedSeparatingAxis.dot(w);
 859
 860                                 // potential exit, they don't overlap
 861                                 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
 862                                 {
 863                                         m_degenerateSimplex = 10;
 864                                         checkSimplex = true;
 865                                         //checkPenetration = false;
 866                                         break;
 867                                 }
 868
 869                                 //exit 0: the new point is already in the simplex, or we didn't come any closer
 870                                 if (m_simplexSolver->inSimplex(w))
 871                                 {
 872                                         m_degenerateSimplex = 1;
 873                                         checkSimplex = true;
 874                                         break;
 875                                 }
 876                                 // are we getting any closer ?
 877                                 btScalar f0 = squaredDistance - delta;
 878                                 btScalar f1 = squaredDistance * REL_ERROR2;
 879
 880                                 if (f0 <= f1)
 881                                 {
 882                                         if (f0 <= btScalar(0.))
 883                                         {
 884                                                 m_degenerateSimplex = 2;
 885                                         }
 886                                         else
 887                                         {
 888                                                 m_degenerateSimplex = 11;
 889                                         }
 890                                         checkSimplex = true;
 891                                         break;
 892                                 }
 893
 894                                 //add current vertex to simplex
 895                                 m_simplexSolver->addVertex(w, pWorld, qWorld);
 896                                 btVector3 newCachedSeparatingAxis;
 897
 898                                 //calculate the closest point to the origin (update vector v)
 899                                 if (!m_simplexSolver->closest(newCachedSeparatingAxis))
 900                                 {
 901                                         m_degenerateSimplex = 3;
 902                                         checkSimplex = true;
 903                                         break;
 904                                 }
 905
 906                                 if (newCachedSeparatingAxis.length2() < REL_ERROR2)
 907                                 {
 908                                         m_cachedSeparatingAxis = newCachedSeparatingAxis;
 909                                         m_degenerateSimplex = 6;
 910                                         checkSimplex = true;
 911                                         break;
 912                                 }
 913
 914                                 btScalar previousSquaredDistance = squaredDistance;
 915                                 squaredDistance = newCachedSeparatingAxis.length2();
 916 #if 0
 917                                 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
 918                                 if (squaredDistance > previousSquaredDistance)
 919                                 {
 920                                         m_degenerateSimplex = 7;
 921                                         squaredDistance = previousSquaredDistance;
 922                                         checkSimplex = false;
 923                                         break;
 924                                 }
 925 #endif  //
 926
 927                                 //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 928
 929                                 //are we getting any closer ?
 930                                 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
 931                                 {
 932                                         //                              m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
 933                                         checkSimplex = true;
 934                                         m_degenerateSimplex = 12;
 935
 936                                         break;
 937                                 }
 938
 939                                 m_cachedSeparatingAxis = newCachedSeparatingAxis;
 940
 941                                 //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
 942                                 if (m_curIter++ > gGjkMaxIter)
 943                                 {
 944 #if defined(DEBUG) || defined(_DEBUG)
 945
 946                                         printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
 947                                         printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
 948                                                    m_cachedSeparatingAxis.getX(),
 949                                                    m_cachedSeparatingAxis.getY(),
 950                                                    m_cachedSeparatingAxis.getZ(),
 951                                                    squaredDistance,
 952                                                    m_minkowskiA->getShapeType(),
 953                                                    m_minkowskiB->getShapeType());
 954
 955 #endif
 956                                         break;
 957                                 }
 958
 959                                 bool check = (!m_simplexSolver->fullSimplex());
 960                                 //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
 961
 962                                 if (!check)
 963                                 {
 964                                         //do we need this backup_closest here ?
 965                                         //                              m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
 966                                         m_degenerateSimplex = 13;
 967                                         break;
 968                                 }
 969                         }
 970
 971                         if (checkSimplex)
 972                         {
 973                                 m_simplexSolver->compute_points(pointOnA, pointOnB);
 974                                 normalInB = m_cachedSeparatingAxis;
 975
 976                                 btScalar lenSqr = m_cachedSeparatingAxis.length2();
 977
 978                                 //valid normal
 979                                 if (lenSqr < REL_ERROR2)
 980                                 {
 981                                         m_degenerateSimplex = 5;
 982                                 }
 983                                 if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
 984                                 {
 985                                         btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 986                                         normalInB *= rlen;  //normalize
 987
 988                                         btScalar s = btSqrt(squaredDistance);
 989
 990                                         btAssert(s > btScalar(0.0));
 991                                         pointOnA -= m_cachedSeparatingAxis * (marginA / s);
 992                                         pointOnB += m_cachedSeparatingAxis * (marginB / s);
 993                                         distance = ((btScalar(1.) / rlen) - margin);
 994                                         isValid = true;
 995                                         orgNormalInB = normalInB;
 996
 997                                         m_lastUsedMethod = 1;
 998                                 }
 999                                 else
1000                                 {
1001                                         m_lastUsedMethod = 2;
1002                                 }
1003                         }
1004                 }
1005
1006                 bool catchDegeneratePenetrationCase =
1007                         (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance));
1008
1009                 //if (checkPenetration && !isValid)
1010                 if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
1011                 {
1012                         //penetration case
1013
1014                         //if there is no way to handle penetrations, bail out
1015                         if (m_penetrationDepthSolver)
1016                         {
1017                                 // Penetration depth case.
1018                                 btVector3 tmpPointOnA, tmpPointOnB;
1019
1020                                 m_cachedSeparatingAxis.setZero();
1021
1022                                 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
1023                                         *m_simplexSolver,
1024                                         m_minkowskiA, m_minkowskiB,
1025                                         localTransA, localTransB,
1026                                         m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
1027                                         debugDraw);
1028
1029                                 if (m_cachedSeparatingAxis.length2())
1030                                 {
1031                                         if (isValid2)
1032                                         {
1033                                                 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
1034                                                 btScalar lenSqr = tmpNormalInB.length2();
1035                                                 if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
1036                                                 {
1037                                                         tmpNormalInB = m_cachedSeparatingAxis;
1038                                                         lenSqr = m_cachedSeparatingAxis.length2();
1039                                                 }
1040
1041                                                 if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
1042                                                 {
1043                                                         tmpNormalInB /= btSqrt(lenSqr);
1044                                                         btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
1045                                                         m_lastUsedMethod = 3;
1046                                                         //only replace valid penetrations when the result is deeper (check)
1047                                                         if (!isValid || (distance2 < distance))
1048                                                         {
1049                                                                 distance = distance2;
1050                                                                 pointOnA = tmpPointOnA;
1051                                                                 pointOnB = tmpPointOnB;
1052                                                                 normalInB = tmpNormalInB;
1053                                                                 isValid = true;
1054                                                         }
1055                                                         else
1056                                                         {
1057                                                                 m_lastUsedMethod = 8;
1058                                                         }
1059                                                 }
1060                                                 else
1061                                                 {
1062                                                         m_lastUsedMethod = 9;
1063                                                 }
1064                                         }
1065                                         else
1066
1067                                         {
1068                                                 ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
1069                                                 ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
1070                                                 ///reports a valid positive distance. Use the results of the second GJK instead of failing.
1071                                                 ///thanks to Jacob.Langford for the reproduction case
1072                                                 ///http://code.google.com/p/bullet/issues/detail?id=250
1073
1074                                                 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
1075                                                 {
1076                                                         btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
1077                                                         //only replace valid distances when the distance is less
1078                                                         if (!isValid || (distance2 < distance))
1079                                                         {
1080                                                                 distance = distance2;
1081                                                                 pointOnA = tmpPointOnA;
1082                                                                 pointOnB = tmpPointOnB;
1083                                                                 pointOnA -= m_cachedSeparatingAxis * marginA;
1084                                                                 pointOnB += m_cachedSeparatingAxis * marginB;
1085                                                                 normalInB = m_cachedSeparatingAxis;
1086                                                                 normalInB.normalize();
1087
1088                                                                 isValid = true;
1089                                                                 m_lastUsedMethod = 6;
1090                                                         }
1091                                                         else
1092                                                         {
1093                                                                 m_lastUsedMethod = 5;
1094                                                         }
1095                                                 }
1096                                         }
1097                                 }
1098                                 else
1099                                 {
1100                                         //printf("EPA didn't return a valid value\n");
1101                                 }
1102                         }
1103                 }
1104         }
1105
1106         if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
1107         {
1108                 m_cachedSeparatingAxis = normalInB;
1109                 m_cachedSeparatingDistance = distance;
1110                 if (1)
1111                 {
1112                         ///todo: need to track down this EPA penetration solver degeneracy
1113                         ///the penetration solver reports penetration but the contact normal
1114                         ///connecting the contact points is pointing in the opposite direction
1115                         ///until then, detect the issue and revert the normal
1116
1117                         btScalar d2 = 0.f;
1118                         {
1119                                 btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
1120                                 btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis();
1121
1122                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1123                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1124
1125                                 btVector3 pWorld = localTransA(pInA);
1126                                 btVector3 qWorld = localTransB(qInB);
1127                                 btVector3 w = pWorld - qWorld;
1128                                 d2 = orgNormalInB.dot(w) - margin;
1129                         }
1130
1131                         btScalar d1 = 0;
1132                         {
1133                                 btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis();
1134                                 btVector3 separatingAxisInB = -normalInB * localTransB.getBasis();
1135
1136                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1137                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1138
1139                                 btVector3 pWorld = localTransA(pInA);
1140                                 btVector3 qWorld = localTransB(qInB);
1141                                 btVector3 w = pWorld - qWorld;
1142                                 d1 = (-normalInB).dot(w) - margin;
1143                         }
1144                         btScalar d0 = 0.f;
1145                         {
1146                                 btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
1147                                 btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis();
1148
1149                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1150                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1151
1152                                 btVector3 pWorld = localTransA(pInA);
1153                                 btVector3 qWorld = localTransB(qInB);
1154                                 btVector3 w = pWorld - qWorld;
1155                                 d0 = normalInB.dot(w) - margin;
1156                         }
1157
1158                         if (d1 > d0)
1159                         {
1160                                 m_lastUsedMethod = 10;
1161                                 normalInB *= -1;
1162                         }
1163
1164                         if (orgNormalInB.length2())
1165                         {
1166                                 if (d2 > d0 && d2 > d1 && d2 > distance)
1167                                 {
1168                                         normalInB = orgNormalInB;
1169                                         distance = d2;
1170                                 }
1171                         }
1172                 }
1173
1174                 output.addContactPoint(
1175                         normalInB,
1176                         pointOnB + positionOffset,
1177                         distance);
1178         }
1179         else
1180         {
1181                 //printf("invalid gjk query\n");
1182         }
1183 }