tesseract_gjk_pair_detector.cpp

Go to the documentation of this file.

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
 
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original
software.
3. This notice may not be removed or altered from any source distribution.
*/
 
#include <tesseract_collision/bullet/tesseract_gjk_pair_detector.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <BulletCollision/CollisionShapes/btConvexShape.h>
#include <BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h>
#include <BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#if defined(DEBUG) || defined(_DEBUG)
//#define TEST_NON_VIRTUAL 1
#include <stdio.h>  //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif  //__SPU__
#endif
 
// must be above the machine epsilon
#ifdef BT_USE_DOUBLE_PRECISION
#define REL_ERROR2 btScalar(1.0e-12)
static const btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
#else
#define REL_ERROR2 btScalar(1.0e-6)
static const btScalar gGjkEpaPenetrationTolerance = 0.001f;
#endif
 
namespace tesseract_collision::tesseract_collision_bullet
{
TesseractGjkPairDetector::TesseractGjkPairDetector(const btConvexShape* objectA,
                                                   const btConvexShape* objectB,
                                                   btSimplexSolverInterface* simplexSolver,
                                                   btConvexPenetrationDepthSolver* penetrationDepthSolver,
                                                   const ContactTestData* cdata)
  : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.))
  , m_penetrationDepthSolver(penetrationDepthSolver)
  , m_simplexSolver(simplexSolver)
  , m_minkowskiA(objectA)
  , m_minkowskiB(objectB)
  , m_shapeTypeA(objectA->getShapeType())
  , m_shapeTypeB(objectB->getShapeType())
  , m_marginA(objectA->getMargin())
  , m_marginB(objectB->getMargin())
  , m_ignoreMargin(false)
  , m_cdata(cdata)
  , m_lastUsedMethod(-1)
  , m_catchDegeneracies(1)
  , m_fixContactNormalDirection(1)
{
}
TesseractGjkPairDetector::TesseractGjkPairDetector(const btConvexShape* objectA,
                                                   const btConvexShape* objectB,
                                                   int shapeTypeA,
                                                   int shapeTypeB,
                                                   btScalar marginA,
                                                   btScalar marginB,
                                                   btSimplexSolverInterface* simplexSolver,
                                                   btConvexPenetrationDepthSolver* penetrationDepthSolver,
                                                   const ContactTestData* cdata)
  : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.))
  , m_penetrationDepthSolver(penetrationDepthSolver)
  , m_simplexSolver(simplexSolver)
  , m_minkowskiA(objectA)
  , m_minkowskiB(objectB)
  , m_shapeTypeA(shapeTypeA)
  , m_shapeTypeB(shapeTypeB)
  , m_marginA(marginA)
  , m_marginB(marginB)
  , m_ignoreMargin(false)
  , m_cdata(cdata)
  , m_lastUsedMethod(-1)
  , m_catchDegeneracies(1)
  , m_fixContactNormalDirection(1)
{
}
 
void TesseractGjkPairDetector::getClosestPoints(const ClosestPointInput& input,
                                                Result& output,
                                                class btIDebugDraw* debugDraw,
                                                bool swapResults)
{
  (void)swapResults;
 
  getClosestPointsNonVirtual(input, output, debugDraw);  // NOLINT
}
 
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)
{
  btVector3 separatingAxisInA = (dir)*localTransA.getBasis();
  btVector3 separatingAxisInB = (-dir) * localTransB.getBasis();
 
  btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
  btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
  btVector3 pInA = pInANoMargin;
  btVector3 qInB = qInBNoMargin;
 
  supAworld = localTransA(pInA);
  supBworld = localTransB(qInB);
 
  if (check2d)
  {
    supAworld[2] = btScalar(0.);
    supBworld[2] = btScalar(0.);
  }
 
  aMinb = supAworld - supBworld;
}
 
struct btSupportVector
{
  btVector3 v;   
  btVector3 v1;  
  btVector3 v2;  
};
 
struct btSimplex
{
  btSupportVector ps[4];  // NOLINT
  int last{ -1 };         
};
 
static const btVector3 ccd_vec3_origin(0, 0, 0);
 
inline void btSimplexInit(btSimplex* s) { s->last = -1; }
 
inline int btSimplexSize(const btSimplex* s) { return s->last + 1; }
 
inline const btSupportVector* btSimplexPoint(const btSimplex* s, int idx)
{
  // here is no check on boundaries
  return &s->ps[idx];
}
inline void btSupportCopy(btSupportVector* d, const btSupportVector* s) { *d = *s; }
 
inline void btVec3Copy(btVector3* v, const btVector3* w) { *v = *w; }
 
inline void ccdVec3Add(btVector3* v, const btVector3* w)
{
  v->m_floats[0] += w->m_floats[0];
  v->m_floats[1] += w->m_floats[1];
  v->m_floats[2] += w->m_floats[2];
}
 
inline void ccdVec3Sub(btVector3* v, const btVector3* w) { *v -= *w; }
inline void btVec3Sub2(btVector3* d, const btVector3* v, const btVector3* w) { *d = (*v) - (*w); }
inline btScalar btVec3Dot(const btVector3* a, const btVector3* b)
{
  btScalar dot = a->dot(*b);
 
  return dot;
}
 
inline btScalar ccdVec3Dist2(const btVector3* a, const btVector3* b)
{
  btVector3 ab;
  btVec3Sub2(&ab, a, b);
  return btVec3Dot(&ab, &ab);
}
 
inline void btVec3Scale(btVector3* d, btScalar k)
{
  d->m_floats[0] *= k;
  d->m_floats[1] *= k;
  d->m_floats[2] *= k;
}
 
inline void btVec3Cross(btVector3* d, const btVector3* a, const btVector3* b)
{
  d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]);
  d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]);
  d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]);
}
 
inline void btTripleCross(const btVector3* a, const btVector3* b, const btVector3* c, btVector3* d)
{
  btVector3 e;
  btVec3Cross(&e, a, b);
  btVec3Cross(d, &e, c);
}
 
inline int ccdEq(btScalar _a, btScalar _b)
{
  btScalar ab = btFabs(_a - _b);
  if (btFabs(ab) < SIMD_EPSILON)
    return 1;
 
  btScalar a = btFabs(_a);
  btScalar b = btFabs(_b);
  if (b > a)
  {
    return ab < SIMD_EPSILON * b;  // NOLINT
  }
  else  // NOLINT
  {
    return ab < SIMD_EPSILON * a;  // NOLINT
  }
}
 
btScalar ccdVec3X(const btVector3* v) { return v->x(); }
 
btScalar ccdVec3Y(const btVector3* v) { return v->y(); }
 
btScalar ccdVec3Z(const btVector3* v) { return v->z(); }
inline int btVec3Eq(const btVector3* a, const btVector3* b)
{
  return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) &&  // NOLINT
         ccdEq(ccdVec3Z(a), ccdVec3Z(b));                                       // NOLINT
}
 
inline void btSimplexAdd(btSimplex* s, const btSupportVector* v)
{
  // here is no check on boundaries in sake of speed
  ++s->last;
  btSupportCopy(s->ps + s->last, v);
}
 
inline void btSimplexSet(btSimplex* s, size_t pos, const btSupportVector* a) { btSupportCopy(s->ps + pos, a); }
 
inline void btSimplexSetSize(btSimplex* s, int size) { s->last = size - 1; }
 
inline const btSupportVector* ccdSimplexLast(const btSimplex* s) { return btSimplexPoint(s, s->last); }
 
inline int ccdSign(btScalar val)
{
  if (btFuzzyZero(val))
  {
    return 0;
  }
  else if (val < btScalar(0))  // NOLINT
  {
    return -1;
  }
  return 1;
}
 
inline btScalar btVec3PointSegmentDist2(const btVector3* P, const btVector3* x0, const btVector3* b, btVector3* witness)
{
  // The computation comes from solving equation of segment:
  //      S(t) = x0 + t.d
  //          where - x0 is initial point of segment
  //                - d is direction of segment from x0 (|d| > 0)
  //                - t belongs to <0, 1> interval
  //
  // Than, distance from a segment to some point P can be expressed:
  //      D(t) = |x0 + t.d - P|^2
  //          which is distance from any point on segment. Minimization
  //          of this function brings distance from P to segment.
  // Minimization of D(t) leads to simple quadratic equation that's
  // solving is straightforward.
  //
  // Bonus of this method is witness point for free.
 
  btScalar dist{ std::numeric_limits<btScalar>::max() }, t{ std::numeric_limits<btScalar>::max() };
  btVector3 d, a;
 
  // direction of segment
  btVec3Sub2(&d, b, x0);
 
  // precompute vector from P to x0
  btVec3Sub2(&a, x0, P);
 
  t = -btScalar(1.) * btVec3Dot(&a, &d);
  t /= btVec3Dot(&d, &d);
 
  if (t < btScalar(0) || btFuzzyZero(t))
  {
    dist = ccdVec3Dist2(x0, P);
    if (witness != nullptr)
      btVec3Copy(witness, x0);
  }
  else if (t > btScalar(1) || ccdEq(t, btScalar(1)))  // NOLINT
  {
    dist = ccdVec3Dist2(b, P);
    if (witness != nullptr)
      btVec3Copy(witness, b);
  }
  else
  {
    if (witness != nullptr)
    {
      btVec3Copy(witness, &d);
      btVec3Scale(witness, t);
      ccdVec3Add(witness, x0);
      dist = ccdVec3Dist2(witness, P);
    }
    else
    {
      // recycling variables
      btVec3Scale(&d, t);
      ccdVec3Add(&d, &a);
      dist = btVec3Dot(&d, &d);
    }
  }
 
  return dist;
}
 
btScalar
btVec3PointTriDist2(const btVector3* P, const btVector3* x0, const btVector3* B, const btVector3* C, btVector3* witness)
{
  // Computation comes from analytic expression for triangle (x0, B, C)
  //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
  // Then equation for distance is:
  //      D(s, t) = | T(s, t) - P |^2
  // This leads to minimization of quadratic function of two variables.
  // The solution from is taken only if s is between 0 and 1, t is
  // between 0 and 1 and t + s < 1, otherwise distance from segment is
  // computed.
 
  btVector3 d1, d2, a;
  double dist{ std::numeric_limits<double>::max() }, dist2{ std::numeric_limits<double>::max() };
  btVector3 witness2;
 
  btVec3Sub2(&d1, B, x0);
  btVec3Sub2(&d2, C, x0);
  btVec3Sub2(&a, x0, P);
 
  double u = btVec3Dot(&a, &a);
  double v = btVec3Dot(&d1, &d1);
  double w = btVec3Dot(&d2, &d2);
  double p = btVec3Dot(&a, &d1);
  double q = btVec3Dot(&a, &d2);
  double r = btVec3Dot(&d1, &d2);
 
  double s = (q * r - w * p) / (w * v - r * r);
  double t = (-s * r - q) / w;
 
  if ((btFuzzyZero(static_cast<btScalar>(s)) || s > 0) && (ccdEq(static_cast<btScalar>(s), 1) || s < 1) &&  // NOLINT
      (btFuzzyZero(static_cast<btScalar>(t)) || t > 0) && (ccdEq(static_cast<btScalar>(t), 1) || t < 1) &&  // NOLINT
      (ccdEq(static_cast<btScalar>(t + s), 1) || static_cast<btScalar>(t + s) < 1))                         // NOLINT
  {
    if (witness != nullptr)
    {
      btVec3Scale(&d1, static_cast<btScalar>(s));
      btVec3Scale(&d2, static_cast<btScalar>(t));
      btVec3Copy(witness, x0);
      ccdVec3Add(witness, &d1);
      ccdVec3Add(witness, &d2);
 
      dist = ccdVec3Dist2(witness, P);
    }
    else
    {
      dist = s * s * v;
      dist += t * t * w;
      dist += btScalar(2.) * s * t * r;
      dist += btScalar(2.) * s * p;
      dist += btScalar(2.) * t * q;
      dist += u;
    }
  }
  else
  {
    dist = btVec3PointSegmentDist2(P, x0, B, witness);
 
    dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
    if (dist2 < dist)
    {
      dist = dist2;
      if (witness != nullptr)
        btVec3Copy(witness, &witness2);
    }
 
    dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
    if (dist2 < dist)
    {
      dist = dist2;
      if (witness != nullptr)
        btVec3Copy(witness, &witness2);
    }
  }
 
  return static_cast<btScalar>(dist);
}
 
static int btDoSimplex2(btSimplex* simplex, btVector3* dir)
{
  btVector3 AB, AO, tmp;
 
  // get last added as A
  const btSupportVector* A = ccdSimplexLast(simplex);
  // get the other point
  const btSupportVector* B = btSimplexPoint(simplex, 0);
  // compute AB oriented segment
  btVec3Sub2(&AB, &B->v, &A->v);
  // compute AO vector
  btVec3Copy(&AO, &A->v);
  btVec3Scale(&AO, -btScalar(1));
 
  // dot product AB . AO
  btScalar dot = btVec3Dot(&AB, &AO);
 
  // check if origin doesn't lie on AB segment
  btVec3Cross(&tmp, &AB, &AO);
  if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0))
  {
    return 1;
  }
 
  // check if origin is in area where AB segment is
  if (btFuzzyZero(dot) || dot < btScalar(0))
  {
    // origin is in outside are of A
    btSimplexSet(simplex, 0, A);
    btSimplexSetSize(simplex, 1);
    btVec3Copy(dir, &AO);
  }
  else
  {
    // origin is in area where AB segment is
 
    // keep simplex untouched and set direction to
    // AB x AO x AB
    btTripleCross(&AB, &AO, &AB, dir);
  }
 
  return 0;
}
 
static int btDoSimplex3(btSimplex* simplex, btVector3* dir)
{
  btVector3 AO, AB, AC, ABC, tmp;
 
  // get last added as A
  const btSupportVector* A = ccdSimplexLast(simplex);
  // get the other points
  const btSupportVector* B = btSimplexPoint(simplex, 1);
  const btSupportVector* C = btSimplexPoint(simplex, 0);
 
  // check touching contact
  btScalar dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
  if (btFuzzyZero(dist))
  {
    return 1;
  }
 
  // check if triangle is really triangle (has area > 0)
  // if not simplex can't be expanded and thus no itersection is found
  if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v))  // NOLINT
  {
    return -1;
  }
 
  // compute AO vector
  btVec3Copy(&AO, &A->v);
  btVec3Scale(&AO, -btScalar(1));
 
  // compute AB and AC segments and ABC vector (perpendircular to triangle)
  btVec3Sub2(&AB, &B->v, &A->v);
  btVec3Sub2(&AC, &C->v, &A->v);
  btVec3Cross(&ABC, &AB, &AC);
 
  btVec3Cross(&tmp, &ABC, &AC);
  btScalar dot = btVec3Dot(&tmp, &AO);
  if (btFuzzyZero(dot) || dot > btScalar(0))
  {
    dot = btVec3Dot(&AC, &AO);
    if (btFuzzyZero(dot) || dot > btScalar(0))
    {
      // C is already in place
      btSimplexSet(simplex, 1, A);
      btSimplexSetSize(simplex, 2);
      btTripleCross(&AC, &AO, &AC, dir);
    }
    else
    {
      dot = btVec3Dot(&AB, &AO);
      if (btFuzzyZero(dot) || dot > btScalar(0))
      {
        btSimplexSet(simplex, 0, B);
        btSimplexSet(simplex, 1, A);
        btSimplexSetSize(simplex, 2);
        btTripleCross(&AB, &AO, &AB, dir);
      }
      else
      {
        btSimplexSet(simplex, 0, A);
        btSimplexSetSize(simplex, 1);
        btVec3Copy(dir, &AO);
      }
    }
  }
  else
  {
    btVec3Cross(&tmp, &AB, &ABC);
    dot = btVec3Dot(&tmp, &AO);
    if (btFuzzyZero(dot) || dot > btScalar(0))
    {
      dot = btVec3Dot(&AB, &AO);
      if (btFuzzyZero(dot) || dot > btScalar(0))
      {
        btSimplexSet(simplex, 0, B);
        btSimplexSet(simplex, 1, A);
        btSimplexSetSize(simplex, 2);
        btTripleCross(&AB, &AO, &AB, dir);
      }
      else
      {
        btSimplexSet(simplex, 0, A);
        btSimplexSetSize(simplex, 1);
        btVec3Copy(dir, &AO);
      }
    }
    else
    {
      dot = btVec3Dot(&ABC, &AO);
      if (btFuzzyZero(dot) || dot > btScalar(0))
      {
        btVec3Copy(dir, &ABC);
      }
      else
      {
        btSupportVector tmp;
        btSupportCopy(&tmp, C);
        btSimplexSet(simplex, 0, B);
        btSimplexSet(simplex, 1, &tmp);
 
        btVec3Copy(dir, &ABC);
        btVec3Scale(dir, -btScalar(1));
      }
    }
  }
 
  return 0;
}
 
static int btDoSimplex4(btSimplex* simplex, btVector3* dir)
{
  btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
 
  // get last added as A
  const btSupportVector* A = ccdSimplexLast(simplex);
  // get the other points
  const btSupportVector* B = btSimplexPoint(simplex, 2);
  const btSupportVector* C = btSimplexPoint(simplex, 1);
  const btSupportVector* D = btSimplexPoint(simplex, 0);
 
  // check if tetrahedron is really tetrahedron (has volume > 0)
  // if it is not simplex can't be expanded and thus no intersection is
  // found
  btScalar dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr);
  if (btFuzzyZero(dist))
  {
    return -1;
  }
 
  // check if origin lies on some of tetrahedron's face - if so objects
  // intersect
  dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
  if (btFuzzyZero(dist))
    return 1;
  dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr);
  if (btFuzzyZero(dist))
    return 1;
  dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr);
  if (btFuzzyZero(dist))
    return 1;
  dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr);
  if (btFuzzyZero(dist))
    return 1;
 
  // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
  btVec3Copy(&AO, &A->v);
  btVec3Scale(&AO, -btScalar(1));
  btVec3Sub2(&AB, &B->v, &A->v);
  btVec3Sub2(&AC, &C->v, &A->v);
  btVec3Sub2(&AD, &D->v, &A->v);
  btVec3Cross(&ABC, &AB, &AC);
  btVec3Cross(&ACD, &AC, &AD);
  btVec3Cross(&ADB, &AD, &AB);
 
  // side (positive or negative) of B, C, D relative to planes ACD, ADB
  // and ABC respectively
  int B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
  int C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
  int D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
 
  // whether origin is on same side of ACD, ADB, ABC as B, C, D
  // respectively
  int AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;  // NOLINT
  int AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;  // NOLINT
  int AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;  // NOLINT
 
  if (AB_O && AC_O && AD_O)  // NOLINT
  {
    // origin is in tetrahedron
    return 1;
    // rearrange simplex to triangle and call btDoSimplex3()
  }
  else if (!AB_O)  // NOLINT
  {
    // B is farthest from the origin among all of the tetrahedron's
    // points, so remove it from the list and go on with the triangle
    // case
 
    // D and C are in place
    btSimplexSet(simplex, 2, A);
    btSimplexSetSize(simplex, 3);
  }
  else if (!AC_O)  // NOLINT
  {
    // C is farthest
    btSimplexSet(simplex, 1, D);
    btSimplexSet(simplex, 0, B);
    btSimplexSet(simplex, 2, A);
    btSimplexSetSize(simplex, 3);
  }
  else
  {  // (!AD_O)
    btSimplexSet(simplex, 0, C);
    btSimplexSet(simplex, 1, B);
    btSimplexSet(simplex, 2, A);
    btSimplexSetSize(simplex, 3);
  }
 
  return btDoSimplex3(simplex, dir);
}
 
static int btDoSimplex(btSimplex* simplex, btVector3* dir)
{
  if (btSimplexSize(simplex) == 2)
  {
    // simplex contains segment only one segment
    return btDoSimplex2(simplex, dir);
  }
  else if (btSimplexSize(simplex) == 3)  // NOLINT
  {
    // simplex contains triangle
    return btDoSimplex3(simplex, dir);
  }
  else
  {  // btSimplexSize(simplex) == 4
    // tetrahedron - this is the only shape which can encapsule origin
    // so btDoSimplex4() also contains test on it
    return btDoSimplex4(simplex, dir);
  }
}
 
#ifdef __SPU__
void TesseractGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,
                                                          Result& output,
                                                          class btIDebugDraw* debugDraw)
#else
void TesseractGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,
                                                          Result& output,
                                                          class btIDebugDraw* debugDraw)
#endif
{
  m_cachedSeparatingDistance = btScalar(0.);
 
  auto distance = btScalar(0.);
  btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
 
  btVector3 pointOnA, pointOnB;
  btTransform localTransA = input.m_transformA;
  btTransform localTransB = input.m_transformB;
  btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
  localTransA.getOrigin() -= positionOffset;
  localTransB.getOrigin() -= positionOffset;
 
  bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
 
  btScalar marginA = m_marginA;
  btScalar marginB = m_marginB;
 
  // for CCD we don't use margins
  if (m_ignoreMargin)
  {
    marginA = btScalar(0.);
    marginB = btScalar(0.);
  }
 
  m_curIter = 0;
  int gGjkMaxIter = 1000;  // this is to catch invalid input, perhaps check for #NaN?
  m_cachedSeparatingAxis.setValue(0, 1, 0);
 
  bool isValid = false;
  bool checkSimplex = false;
  bool checkPenetration = true;
  m_degenerateSimplex = 0;
 
  m_lastUsedMethod = -1;
  int status = -2;
  btVector3 orgNormalInB(0, 0, 0);
  btScalar margin = marginA + marginB;
 
  // we add a separate implementation to check if the convex shapes intersect
  // See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
  // Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
  // and remove this temporary code from libCCD
  // this fixes issue https://github.com/bulletphysics/bullet3/issues/1703
  // note, for large differences in shapes, use double precision build!
  {
    btScalar squaredDistance = BT_LARGE_FLOAT;
    auto delta = btScalar(0.);
 
    btSimplex simplex1;
    btSimplex* simplex = &simplex1;
    btSimplexInit(simplex);
 
    btVector3 dir(1, 0, 0);
 
    {
      btVector3 lastSupV;
      btVector3 supAworld;
      btVector3 supBworld;
      btComputeSupport(
          m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 
      btSupportVector last;
      last.v = lastSupV;
      last.v1 = supAworld;
      last.v2 = supBworld;
 
      btSimplexAdd(simplex, &last);
 
      dir = -lastSupV;
 
      // start iterations
      for (int iterations = 0; iterations < gGjkMaxIter; iterations++)
      {
        // obtain support point
        btComputeSupport(
            m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 
        // check if farthest point in Minkowski difference in direction dir
        // isn't somewhere before origin (the test on negative dot product)
        // - because if it is, objects are not intersecting at all.
        btScalar delta = lastSupV.dot(dir);
        if (delta < 0)
        {
          // no intersection, besides margin
          status = -1;
          break;
        }
 
        // add last support vector to simplex
        last.v = lastSupV;
        last.v1 = supAworld;
        last.v2 = supBworld;
 
        btSimplexAdd(simplex, &last);
 
        // if btDoSimplex returns 1 if objects intersect, -1 if objects don't
        // intersect and 0 if algorithm should continue
 
        btVector3 newDir;
        int do_simplex_res = btDoSimplex(simplex, &dir);
 
        if (do_simplex_res == 1)
        {
          status = 0;  // intersection found
          break;
        }
        else if (do_simplex_res == -1)  // NOLINT
        {
          // intersection not found
          status = -1;
          break;
        }
 
        if (btFuzzyZero(btVec3Dot(&dir, &dir)))
        {
          // intersection not found
          status = -1;
        }
 
        if (dir.length2() < SIMD_EPSILON)
        {
          // no intersection, besides margin
          status = -1;
          break;
        }
 
        if (dir.fuzzyZero())
        {
          // intersection not found
          status = -1;
          break;
        }
      }
    }
 
    if (status == 0 && !m_cdata->req.calculate_penetration)
    {
      // The shapes intersect but penetration was not request so return empty contact
      output.addContactPoint(normalInB, pointOnB + positionOffset, -std::numeric_limits<btScalar>::min());
      return;
    }
 
    if (status == -1 && !m_cdata->req.calculate_distance)
    {
      // The shapes do not intersect and we did not request distance data so return.
      return;
    }
 
    m_simplexSolver->reset();
    // printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
    if (1)  // NOLINT
    {
      for (;;)
      // while (true)
      {
        btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
        btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
 
        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
        btVector3 pWorld = localTransA(pInA);
        btVector3 qWorld = localTransB(qInB);
 
        if (check2d)
        {
          pWorld[2] = btScalar(0.);
          qWorld[2] = btScalar(0.);
        }
 
        btVector3 w = pWorld - qWorld;
        delta = m_cachedSeparatingAxis.dot(w);
 
        // potential exit, they don't overlap
        if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
        {
          m_degenerateSimplex = 10;
          checkSimplex = true;
          // checkPenetration = false;
          break;
        }
 
        // exit 0: the new point is already in the simplex, or we didn't come any closer
        if (m_simplexSolver->inSimplex(w))
        {
          m_degenerateSimplex = 1;
          checkSimplex = true;
          break;
        }
        // are we getting any closer ?
        btScalar f0 = squaredDistance - delta;
        btScalar f1 = squaredDistance * REL_ERROR2;
 
        if (f0 <= f1)
        {
          if (f0 <= btScalar(0.))
          {
            m_degenerateSimplex = 2;
          }
          else
          {
            m_degenerateSimplex = 11;
          }
          checkSimplex = true;
          break;
        }
 
        // add current vertex to simplex
        m_simplexSolver->addVertex(w, pWorld, qWorld);
        btVector3 newCachedSeparatingAxis;
 
        // calculate the closest point to the origin (update vector v)
        if (!m_simplexSolver->closest(newCachedSeparatingAxis))
        {
          m_degenerateSimplex = 3;
          checkSimplex = true;
          break;
        }
 
        if (newCachedSeparatingAxis.length2() < REL_ERROR2)
        {
          m_cachedSeparatingAxis = newCachedSeparatingAxis;
          m_degenerateSimplex = 6;
          checkSimplex = true;
          break;
        }
 
        btScalar previousSquaredDistance = squaredDistance;
        squaredDistance = newCachedSeparatingAxis.length2();
#if 0   // NOLINT
        if (squaredDistance > previousSquaredDistance)
        {
          m_degenerateSimplex = 7;
          squaredDistance = previousSquaredDistance;
          checkSimplex = false;
          break;
        }
#endif  //
 
        // redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 
        // are we getting any closer ?
        if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
        {
          //                            m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
          checkSimplex = true;
          m_degenerateSimplex = 12;
 
          break;
        }
 
        m_cachedSeparatingAxis = newCachedSeparatingAxis;
 
        // degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
        if (m_curIter++ > gGjkMaxIter)
        {
#if defined(DEBUG) || defined(_DEBUG)
 
          printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
          printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
                 m_cachedSeparatingAxis.getX(),
                 m_cachedSeparatingAxis.getY(),
                 m_cachedSeparatingAxis.getZ(),
                 squaredDistance,
                 m_minkowskiA->getShapeType(),
                 m_minkowskiB->getShapeType());
 
#endif
          break;
        }
 
        bool check = (!m_simplexSolver->fullSimplex());
        // bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON *
        // m_simplexSolver->maxVertex());
 
        if (!check)
        {
          // do we need this backup_closest here ?
          //                            m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
          m_degenerateSimplex = 13;
          break;
        }
      }
 
      if (checkSimplex)
      {
        m_simplexSolver->compute_points(pointOnA, pointOnB);
        normalInB = m_cachedSeparatingAxis;
 
        btScalar lenSqr = m_cachedSeparatingAxis.length2();
 
        // valid normal
        if (lenSqr < REL_ERROR2)
        {
          m_degenerateSimplex = 5;
        }
        if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
        {
          btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
          normalInB *= rlen;  // normalize
 
          btScalar s = btSqrt(squaredDistance);
 
          btAssert(s > btScalar(0.0));
          pointOnA -= m_cachedSeparatingAxis * (marginA / s);
          pointOnB += m_cachedSeparatingAxis * (marginB / s);
          distance = ((btScalar(1.) / rlen) - margin);
          isValid = true;
          orgNormalInB = normalInB;
 
          m_lastUsedMethod = 1;
        }
        else
        {
          m_lastUsedMethod = 2;
        }
      }
    }
 
    bool catchDegeneratePenetrationCase =
        (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex &&  // NOLINT
         ((distance + margin) < gGjkEpaPenetrationTolerance));
 
    // if (checkPenetration && !isValid)
    if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
    {
      // penetration case
 
      // if there is no way to handle penetrations, bail out
      if (m_penetrationDepthSolver != nullptr)
      {
        // Penetration depth case.
        btVector3 tmpPointOnA, tmpPointOnB;
 
        m_cachedSeparatingAxis.setZero();
 
        bool isValid2 = m_penetrationDepthSolver->calcPenDepth(*m_simplexSolver,
                                                               m_minkowskiA,
                                                               m_minkowskiB,
                                                               localTransA,
                                                               localTransB,
                                                               m_cachedSeparatingAxis,
                                                               tmpPointOnA,
                                                               tmpPointOnB,
                                                               debugDraw);
 
        if (m_cachedSeparatingAxis.length2() > 0)
        {
          if (isValid2)
          {
            btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
            btScalar lenSqr = tmpNormalInB.length2();
            if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
            {
              tmpNormalInB = m_cachedSeparatingAxis;
              lenSqr = m_cachedSeparatingAxis.length2();
            }
 
            if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
            {
              tmpNormalInB /= btSqrt(lenSqr);
              btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
              m_lastUsedMethod = 3;
              // only replace valid penetrations when the result is deeper (check)
              if (!isValid || (distance2 < distance))
              {
                distance = distance2;
                pointOnA = tmpPointOnA;
                pointOnB = tmpPointOnB;
                normalInB = tmpNormalInB;
                isValid = true;
              }
              else
              {
                m_lastUsedMethod = 8;
              }
            }
            else
            {
              m_lastUsedMethod = 9;
            }
          }
          else
 
          {
 
            if (m_cachedSeparatingAxis.length2() > btScalar(0.))
            {
              btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
              // only replace valid distances when the distance is less
              if (!isValid || (distance2 < distance))
              {
                distance = distance2;
                pointOnA = tmpPointOnA;
                pointOnB = tmpPointOnB;
                pointOnA -= m_cachedSeparatingAxis * marginA;
                pointOnB += m_cachedSeparatingAxis * marginB;
                normalInB = m_cachedSeparatingAxis;
                normalInB.normalize();
 
                isValid = true;
                m_lastUsedMethod = 6;
              }
              else
              {
                m_lastUsedMethod = 5;
              }
            }
          }
        }
        else
        {
          // printf("EPA didn't return a valid value\n");
        }
      }
    }
  }
 
  if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
  {
    m_cachedSeparatingAxis = normalInB;
    m_cachedSeparatingDistance = distance;
    if (1)  // NOLINT
    {
 
      auto d2 = btScalar(0.);
      {
        btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
        btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis();
 
        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
        btVector3 pWorld = localTransA(pInA);
        btVector3 qWorld = localTransB(qInB);
        btVector3 w = pWorld - qWorld;
        d2 = orgNormalInB.dot(w) - margin;
      }
 
      btScalar d1 = 0;
      {
        btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis();
        btVector3 separatingAxisInB = -normalInB * localTransB.getBasis();
 
        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
        btVector3 pWorld = localTransA(pInA);
        btVector3 qWorld = localTransB(qInB);
        btVector3 w = pWorld - qWorld;
        d1 = (-normalInB).dot(w) - margin;
      }
      auto d0 = btScalar(0.);
      {
        btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
        btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis();
 
        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
        btVector3 pWorld = localTransA(pInA);
        btVector3 qWorld = localTransB(qInB);
        btVector3 w = pWorld - qWorld;
        d0 = normalInB.dot(w) - margin;
      }
 
      if (d1 > d0)
      {
        m_lastUsedMethod = 10;
        normalInB *= -1;
      }
 
      if (orgNormalInB.length2() > 0)
      {
        if (d2 > d0 && d2 > d1 && d2 > distance)
        {
          normalInB = orgNormalInB;
          distance = d2;
        }
      }
    }
 
    output.addContactPoint(normalInB, pointOnB + positionOffset, distance);
  }
  else
  {
    // printf("invalid gjk query\n");
  }
}
}  // namespace tesseract_collision::tesseract_collision_bullet