GJK fix for https://github.com/bulletphysics/bullet3/issues/1703

In a nutshell, we added a more reliable check, based on if the origin is in the GJK simplex, to determine if we are really intersecting and need to run EPA.
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
Note, for large differences in shapes, use double precision build!

src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

@@ -33,8 +33,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
 	(void)simplexSolver;
 
 	btVector3 guessVectors[] = {
-		btVector3(transformB.getOrigin() - transformA.getOrigin()),
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
-		btVector3(transformA.getOrigin() - transformB.getOrigin()),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
 		btVector3(0, 0, 1),
 		btVector3(0, 1, 0),
 		btVector3(1, 0, 0),

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -1,4 +1,4 @@
-/*
+/*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
@@ -83,6 +83,593 @@ void	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 	getClosestPointsNonVirtual(input,output,debugDraw);
 }
 
+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 seperatingAxisInA = (dir)* localTransA.getBasis();
+	btVector3 seperatingAxisInB = (-dir)* localTransB.getBasis();
+
+	btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+	btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+	btVector3 pInA = pInANoMargin;
+	btVector3 qInB = qInBNoMargin;
+
+	supAworld = localTransA(pInA);
+	supBworld = localTransB(qInB);
+
+	if (check2d)
+	{
+		supAworld[2] = 0.f;
+		supBworld[2] = 0.f;
+	}
+
+	aMinb = supAworld - supBworld;
+}
+
+struct btSupportVector 
+{
+	btVector3 v;  //!< Support point in minkowski sum
+	btVector3 v1; //!< Support point in obj1
+	btVector3 v2; //!< Support point in obj2
+};
+
+struct btSimplex 
+{
+	btSupportVector ps[4];
+	int last; //!< index of last added point
+};
+
+static 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;
+	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;
+	btScalar a, b;
+
+	ab = btFabs(_a - _b);
+	if (btFabs(ab) < SIMD_EPSILON)
+		return 1;
+
+	a = btFabs(_a);
+	b = btFabs(_b);
+	if (b > a) {
+		return ab < SIMD_EPSILON * b;
+	}
+	else {
+		return ab < SIMD_EPSILON * a;
+	}
+}
+
+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))
+		&& ccdEq(ccdVec3Z(a), ccdVec3Z(b));
+}
+
+
+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)) {
+		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, t;
+	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)
+			btVec3Copy(witness, x0);
+	}
+	else if (t > btScalar(1) || ccdEq(t, btScalar(1))) {
+		dist = ccdVec3Dist2(b, P);
+		if (witness)
+			btVec3Copy(witness, b);
+	}
+	else {
+		if (witness) {
+			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 u, v, w, p, q, r;
+	double s, t, dist, dist2;
+	btVector3 witness2;
+
+	btVec3Sub2(&d1, B, x0);
+	btVec3Sub2(&d2, C, x0);
+	btVec3Sub2(&a, x0, P);
+
+	u = btVec3Dot(&a, &a);
+	v = btVec3Dot(&d1, &d1);
+	w = btVec3Dot(&d2, &d2);
+	p = btVec3Dot(&a, &d1);
+	q = btVec3Dot(&a, &d2);
+	r = btVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	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))) {
+
+		if (witness) 
+		{
+			btVec3Scale(&d1, s);
+			btVec3Scale(&d2, 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)
+				btVec3Copy(witness, &witness2);
+		}
+
+		dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist) {
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+
+static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B;
+	btVector3 AB, AO, tmp;
+	btScalar dot;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other point
+	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
+	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)
+{
+	const btSupportVector *A, *B, *C;
+	btVector3 AO, AB, AC, ABC, tmp;
+	btScalar dot, dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 1);
+	C = btSimplexPoint(simplex, 0);
+
+	// check touching contact
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	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)) {
+		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);
+	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)
+{
+	const btSupportVector *A, *B, *C, *D;
+	btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
+	int B_on_ACD, C_on_ADB, D_on_ABC;
+	int AB_O, AC_O, AD_O;
+	btScalar dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 2);
+	C = btSimplexPoint(simplex, 1);
+	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
+	dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
+	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, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0);
+	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
+	B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
+	C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
+	D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
+
+	// whether origin is on same side of ACD, ADB, ABC as B, C, D
+	// respectively
+	AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;
+	AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
+	AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
+
+	if (AB_O && AC_O && AD_O) {
+		// origin is in tetrahedron
+		return 1;
+		// rearrange simplex to triangle and call btDoSimplex3()
+	}
+	else if (!AB_O) {
+		// 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) {
+		// 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) {
+		// 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 btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
 #else
@@ -125,23 +712,132 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	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;
 		btScalar delta = btScalar(0.);
 
-		btScalar margin = marginA + marginB;
 
 
 		
+		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) 
+				{
+					// 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;
+				}
+			}
+
+		}
+
 		m_simplexSolver->reset();
+		if (status == 0)
+		{
+			//status = 0;
+			//printf("Intersect!\n");
+		}
 
+		if (status==-1)
+		{
+			//printf("not intersect\n");
+		}
+		//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
+		if (1)
+		{
 			for (; ; )
 				//while (true)
 			{
 
-			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+				btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
-			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+				btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
 
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
@@ -185,7 +881,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					if (f0 <= btScalar(0.))
 					{
 						m_degenerateSimplex = 2;
-				} else
+					}
+					else
 					{
 						m_degenerateSimplex = 11;
 					}
@@ -297,19 +994,24 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					pointOnB += m_cachedSeparatingAxis * (marginB / s);
 					distance = ((btScalar(1.) / rlen) - margin);
 					isValid = true;
+					orgNormalInB = normalInB;
 
 					m_lastUsedMethod = 1;
-			} else
+				}
+				else
 				{
 					m_lastUsedMethod = 2;
 				}
 			}
+		}
+
+
 		
 		bool catchDegeneratePenetrationCase = 
 			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
-		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0))
 		{
 			//penetration case
 
@@ -331,6 +1033,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					);
 
 
+				if (m_cachedSeparatingAxis.length2())
+				{
 					if (isValid2)
 					{
 						btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
@@ -353,18 +1057,20 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 								pointOnA = tmpPointOnA;
 								pointOnB = tmpPointOnB;
 								normalInB = tmpNormalInB;
-							
 								isValid = true;
 
-						} else
+							}
+							else
 							{
 								m_lastUsedMethod = 8;
 							}
-					} else
+						}
+						else
 						{
 							m_lastUsedMethod = 9;
 						}
-				} else
+					}
+					else
 
 					{
 						///this is another degenerate case, where the initial GJK calculation reports a degenerate case
@@ -390,12 +1096,17 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 								isValid = true;
 								m_lastUsedMethod = 6;
-						} else
+							}
+							else
 							{
 								m_lastUsedMethod = 5;
 							}
 						}
 					}
+				} else
+				{
+					//printf("EPA didn't return a valid value\n");
+				}
 				
 			}
 
@@ -409,17 +1120,17 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
-
+		if (1)
 		{
 		///todo: need to track down this EPA penetration solver degeneracy
 		///the penetration solver reports penetration but the contact normal
 		///connecting the contact points is pointing in the opposite direction
 		///until then, detect the issue and revert the normal
 
-			btScalar d1=0;
+			btScalar d2 = 0.f;
 			{
-				btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
+				btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis();
-				btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
+				btVector3 seperatingAxisInB = orgNormalInB* localTransB.getBasis();
 
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
@@ -428,7 +1139,24 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				btVector3  pWorld = localTransA(pInA);
 				btVector3  qWorld = localTransB(qInB);
 				btVector3 w = pWorld - qWorld;
-				d1 = (-normalInB).dot(w);
+				d2 = orgNormalInB.dot(w)- margin;
+			}
+			
+			btScalar d1=0;
+			{
+				
+				btVector3 seperatingAxisInA = (normalInB)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = -normalInB* localTransB.getBasis();
+			
+
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+				btVector3  pWorld = localTransA(pInA);	
+				btVector3  qWorld = localTransB(qInB);
+				btVector3 w	= pWorld - qWorld;
+				d1 = (-normalInB).dot(w)- margin;
+
 			}
 			btScalar d0 = 0.f;
 			{
@@ -442,21 +1170,37 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				btVector3  pWorld = localTransA(pInA);	
 				btVector3  qWorld = localTransB(qInB);
 				btVector3 w	= pWorld - qWorld;
-				d0 = normalInB.dot(w);
+				d0 = normalInB.dot(w)-margin;
 			}
+			
 			if (d1>d0)
 			{
 				m_lastUsedMethod = 10;
 				normalInB*=-1;
 			} 
 
+			if (orgNormalInB.length2())
+			{
+				if (d2 > d0 && d2 > d1 && d2 > distance)
+				{
+
+					normalInB = orgNormalInB;
+					distance = d2;
 				}
+			}
+		}
+
+
 		output.addContactPoint(
 			normalInB,
 			pointOnB+positionOffset,
 			distance);
 
 	}
+	else
+	{
+		//printf("invalid gjk query\n");
+	}
 
 
 }