godot/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

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

//temp globals, to improve GJK/EPA/penetration calculations
int gNumDeepPenetrationChecks = 0;
int gNumGjkChecks = 0;


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,class btIDebugDraw* debugDraw,bool swapResults)
{
	(void)swapResults;

	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
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
#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;

	bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();

	btScalar marginA = m_marginA;
	btScalar marginB = m_marginB;

	gNumGjkChecks++;

	//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;
		btScalar 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) 
				{
					// 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)* localTransA.getBasis();
				btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();


				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);

				btVector3  pWorld = localTransA(pInA);
				btVector3  qWorld = localTransB(qInB);


				if (check2d)
				{
					pWorld[2] = 0.f;
					qWorld[2] = 0.f;
				}

				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
				///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
				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 && ((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)
			{
				// Penetration depth case.
				btVector3 tmpPointOnA,tmpPointOnB;
				
				gNumDeepPenetrationChecks++;
				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())
				{
					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

					{
						///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;
							}
						}
					}
				} 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)
		{
		///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 d2 = 0.f;
			{
				btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis();
				btVector3 seperatingAxisInB = orgNormalInB* 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;
				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;
			{
				btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
				btVector3 seperatingAxisInB = normalInB* 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;
				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");
	}


}