/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org

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 "btGjkPairDetector.h"
#include "../CollisionShapes/btConvexShape.h"
#include "../NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "../NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"

#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)
btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
#else
#define REL_ERROR2 btScalar(1.0e-6)
btScalar gGjkEpaPenetrationTolerance = 0.001;
#endif


btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
	: 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_lastUsedMethod(-1),
	  m_catchDegeneracies(1),
	  m_fixContactNormalDirection(1)
{
}
btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
	: 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_lastUsedMethod(-1),
	  m_catchDegeneracies(1),
	  m_fixContactNormalDirection(1)
{
}

void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, bool swapResults)
{
	(void)swapResults;

	getClosestPointsNonVirtual(input, output);
}

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] = 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;
	btScalar u, v, w, p, q, r;
	btScalar 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)
#else
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output)
#endif
{
	m_cachedSeparatingDistance = 0.f;

	btScalar 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;

	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;

	{
		btScalar squaredDistance = BT_LARGE_FLOAT;
		btScalar delta = btScalar(0.);

		btScalar margin = marginA + marginB;

		m_simplexSolver->reset();

		while (true)
		{

			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA.getBasis();
			btVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB.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;
			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();


			//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) {
				break;

			}

			bool check = (!m_simplexSolver->fullSimplex());
			if (!check) {
				m_degenerateSimplex = 13;
				break;
			}
		}

		if (checkSimplex) {
			m_simplexSolver->compute_points(pointOnA, pointOnB);
			normalInB = m_cachedSeparatingAxis;
			btScalar lenSqr = m_cachedSeparatingAxis.length2();

			//valid normal
			if (lenSqr < 0.0001) {
				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;

				m_lastUsedMethod = 1;
		} else {
				m_lastUsedMethod = 2;
			}
	}

		bool catchDegeneratePenetrationCase =
			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < 0.01));

		//if (checkPenetration && !isValid)
		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) {
			//penetration case

			//if there is no way to handle penetrations, bail out
			if (m_penetrationDepthSolver) {
				// 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
				);


				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();
						//only replace valid penetrations when the result is deeper (check)
						if (!isValid || (distance2 < distance)) {
							distance = distance2;
							pointOnA = tmpPointOnA;
							pointOnB = tmpPointOnB;
							normalInB = tmpNormalInB;
							isValid = true;
							m_lastUsedMethod = 3;
						} else {
							m_lastUsedMethod = 8;
						}
					} else {
						m_lastUsedMethod = 9;
					}
				} else

				{
					///this is another degenerate case, where the initial GJK calculation reports a degenerate case
					///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
					///reports a valid positive distance. Use the results of the second GJK instead of failing.
					///thanks to Jacob.Langford for the reproduction case
					///http://code.google.com/p/bullet/issues/detail?id=250


					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;
						}
					}
				}

}

		}
	}

	if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared))) {

		if (m_fixContactNormalDirection) {
			///@workaround for sticky convex collisions
			//in some degenerate cases (usually when the use uses very small margins) 
			//the contact normal is pointing the wrong direction
			//so fix it now (until we can deal with all degenerate cases in GJK and EPA)
			//contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
			//We like to use a dot product of the normal against the difference of the centroids, 
			//once the centroid is available in the API
			//until then we use the center of the aabb to approximate the centroid
			btVector3 aabbMin, aabbMax;
			m_minkowskiA->getAabb(localTransA, aabbMin, aabbMax);
			btVector3 posA = (aabbMax + aabbMin) * btScalar(0.5);

			m_minkowskiB->getAabb(localTransB, aabbMin, aabbMax);
			btVector3 posB = (aabbMin + aabbMax) * btScalar(0.5);

			btVector3 diff = posA - posB;
			if (diff.dot(normalInB) < 0.f)
				normalInB *= -1.f;
		}
		m_cachedSeparatingAxis = normalInB;
		m_cachedSeparatingDistance = distance;

		output.addContactPoint(
			normalInB,
			pointOnB + positionOffset,
			distance);

	}
}