/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 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.
*/

/*
GJK-EPA collision solver by Nathanael Presson, 2008
*/
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "btGjkEpa2.h"

#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
#endif

namespace gjkepa2_impl
{

	// Config

	/* GJK	*/ 
#define GJK_MAX_ITERATIONS	128

#ifdef BT_USE_DOUBLE_PRECISION
	#define GJK_ACCURACY		((btScalar)1e-12)
	#define GJK_MIN_DISTANCE	((btScalar)1e-12)
	#define GJK_DUPLICATED_EPS	((btScalar)1e-12)
#else
	#define GJK_ACCURACY		((btScalar)0.0001)
	#define GJK_MIN_DISTANCE	((btScalar)0.0001)
	#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
#endif //BT_USE_DOUBLE_PRECISION


#define GJK_SIMPLEX2_EPS	((btScalar)0.0)
#define GJK_SIMPLEX3_EPS	((btScalar)0.0)
#define GJK_SIMPLEX4_EPS	((btScalar)0.0)

	/* EPA	*/ 
#define EPA_MAX_VERTICES	128
#define EPA_MAX_ITERATIONS	255

#ifdef BT_USE_DOUBLE_PRECISION
	#define EPA_ACCURACY		((btScalar)1e-12)
	#define EPA_PLANE_EPS		((btScalar)1e-14)
	#define EPA_INSIDE_EPS		((btScalar)1e-9)
#else
	#define EPA_ACCURACY		((btScalar)0.0001)
	#define EPA_PLANE_EPS		((btScalar)0.00001)
	#define EPA_INSIDE_EPS		((btScalar)0.01)
#endif

#define EPA_FALLBACK            (10*EPA_ACCURACY)
#define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)


	// Shorthands
	typedef unsigned int	U;
	typedef unsigned char	U1;

	// MinkowskiDiff
	struct	MinkowskiDiff
	{
		const btConvexShape*	m_shapes[2];
		btMatrix3x3				m_toshape1;
		btTransform				m_toshape0;
#ifdef __SPU__
		bool					m_enableMargin;
#else
		btVector3				(btConvexShape::*Ls)(const btVector3&) const;
#endif//__SPU__
		

		MinkowskiDiff()
		{

		}
#ifdef __SPU__
			void					EnableMargin(bool enable)
		{
			m_enableMargin = enable;
		}	
		inline btVector3		Support0(const btVector3& d) const
		{
			if (m_enableMargin)
			{
				return m_shapes[0]->localGetSupportVertexNonVirtual(d);
			} else
			{
				return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
			}
		}
		inline btVector3		Support1(const btVector3& d) const
		{
			if (m_enableMargin)
			{
				return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
			} else
			{
				return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
			}
		}
#else
		void					EnableMargin(bool enable)
		{
			if(enable)
				Ls=&btConvexShape::localGetSupportVertexNonVirtual;
			else
				Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
		}	
		inline btVector3		Support0(const btVector3& d) const
		{
			return(((m_shapes[0])->*(Ls))(d));
		}
		inline btVector3		Support1(const btVector3& d) const
		{
			return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
		}
#endif //__SPU__

		inline btVector3		Support(const btVector3& d) const
		{
			return(Support0(d)-Support1(-d));
		}
		btVector3				Support(const btVector3& d,U index) const
		{
			if(index)
				return(Support1(d));
			else
				return(Support0(d));
		}
	};

	typedef	MinkowskiDiff	tShape;


	// GJK
	struct	GJK
	{
		/* Types		*/ 
		struct	sSV
		{
			btVector3	d,w;
		};
		struct	sSimplex
		{
			sSV*		c[4];
			btScalar	p[4];
			U			rank;
		};
		struct	eStatus	{ enum _ {
			Valid,
			Inside,
			Failed		};};
			/* Fields		*/ 
			tShape			m_shape;
			btVector3		m_ray;
			btScalar		m_distance;
			sSimplex		m_simplices[2];
			sSV				m_store[4];
			sSV*			m_free[4];
			U				m_nfree;
			U				m_current;
			sSimplex*		m_simplex;
			eStatus::_		m_status;
			/* Methods		*/ 
			GJK()
			{
				Initialize();
			}
			void				Initialize()
			{
				m_ray		=	btVector3(0,0,0);
				m_nfree		=	0;
				m_status	=	eStatus::Failed;
				m_current	=	0;
				m_distance	=	0;
			}
			eStatus::_			Evaluate(const tShape& shapearg,const btVector3& guess)
			{
				U			iterations=0;
				btScalar	sqdist=0;
				btScalar	alpha=0;
				btVector3	lastw[4];
				U			clastw=0;
				/* Initialize solver		*/ 
				m_free[0]			=	&m_store[0];
				m_free[1]			=	&m_store[1];
				m_free[2]			=	&m_store[2];
				m_free[3]			=	&m_store[3];
				m_nfree				=	4;
				m_current			=	0;
				m_status			=	eStatus::Valid;
				m_shape				=	shapearg;
				m_distance			=	0;
				/* Initialize simplex		*/ 
				m_simplices[0].rank	=	0;
				m_ray				=	guess;
				const btScalar	sqrl=	m_ray.length2();
				appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
				m_simplices[0].p[0]	=	1;
				m_ray				=	m_simplices[0].c[0]->w;	
				sqdist				=	sqrl;
				lastw[0]			=
					lastw[1]			=
					lastw[2]			=
					lastw[3]			=	m_ray;
				/* Loop						*/ 
				do	{
					const U		next=1-m_current;
					sSimplex&	cs=m_simplices[m_current];
					sSimplex&	ns=m_simplices[next];
					/* Check zero							*/ 
					const btScalar	rl=m_ray.length();
					if(rl<GJK_MIN_DISTANCE)
					{/* Touching or inside				*/ 
						m_status=eStatus::Inside;
						break;
					}
					/* Append new vertice in -'v' direction	*/ 
					appendvertice(cs,-m_ray);
					const btVector3&	w=cs.c[cs.rank-1]->w;
					bool				found=false;
					for(U i=0;i<4;++i)
					{
						if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
						{ found=true;break; }
					}
					if(found)
					{/* Return old simplex				*/ 
						removevertice(m_simplices[m_current]);
						break;
					}
					else
					{/* Update lastw					*/ 
						lastw[clastw=(clastw+1)&3]=w;
					}
					/* Check for termination				*/ 
					const btScalar	omega=btDot(m_ray,w)/rl;
					alpha=btMax(omega,alpha);
					if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
					{/* Return old simplex				*/ 
						removevertice(m_simplices[m_current]);
						break;
					}		
					/* Reduce simplex						*/ 
					btScalar	weights[4];
					U			mask=0;
					switch(cs.rank)
					{
					case	2:	sqdist=projectorigin(	cs.c[0]->w,
									cs.c[1]->w,
									weights,mask);break;
					case	3:	sqdist=projectorigin(	cs.c[0]->w,
									cs.c[1]->w,
									cs.c[2]->w,
									weights,mask);break;
					case	4:	sqdist=projectorigin(	cs.c[0]->w,
									cs.c[1]->w,
									cs.c[2]->w,
									cs.c[3]->w,
									weights,mask);break;
					}
					if(sqdist>=0)
					{/* Valid	*/ 
						ns.rank		=	0;
						m_ray		=	btVector3(0,0,0);
						m_current	=	next;
						for(U i=0,ni=cs.rank;i<ni;++i)
						{
							if(mask&(1<<i))
							{
								ns.c[ns.rank]		=	cs.c[i];
								ns.p[ns.rank++]		=	weights[i];
								m_ray				+=	cs.c[i]->w*weights[i];
							}
							else
							{
								m_free[m_nfree++]	=	cs.c[i];
							}
						}
						if(mask==15) m_status=eStatus::Inside;
					}
					else
					{/* Return old simplex				*/ 
						removevertice(m_simplices[m_current]);
						break;
					}
					m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
				} while(m_status==eStatus::Valid);
				m_simplex=&m_simplices[m_current];
				switch(m_status)
				{
				case	eStatus::Valid:		m_distance=m_ray.length();break;
				case	eStatus::Inside:	m_distance=0;break;
				default:
					{
					}
				}	
				return(m_status);
			}
			bool					EncloseOrigin()
			{
				switch(m_simplex->rank)
				{
				case	1:
					{
						for(U i=0;i<3;++i)
						{
							btVector3		axis=btVector3(0,0,0);
							axis[i]=1;
							appendvertice(*m_simplex, axis);
							if(EncloseOrigin())	return(true);
							removevertice(*m_simplex);
							appendvertice(*m_simplex,-axis);
							if(EncloseOrigin())	return(true);
							removevertice(*m_simplex);
						}
					}
					break;
				case	2:
					{
						const btVector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
						for(U i=0;i<3;++i)
						{
							btVector3		axis=btVector3(0,0,0);
							axis[i]=1;
							const btVector3	p=btCross(d,axis);
							if(p.length2()>0)
							{
								appendvertice(*m_simplex, p);
								if(EncloseOrigin())	return(true);
								removevertice(*m_simplex);
								appendvertice(*m_simplex,-p);
								if(EncloseOrigin())	return(true);
								removevertice(*m_simplex);
							}
						}
					}
					break;
				case	3:
					{
						const btVector3	n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
							m_simplex->c[2]->w-m_simplex->c[0]->w);
						if(n.length2()>0)
						{
							appendvertice(*m_simplex,n);
							if(EncloseOrigin())	return(true);
							removevertice(*m_simplex);
							appendvertice(*m_simplex,-n);
							if(EncloseOrigin())	return(true);
							removevertice(*m_simplex);
						}
					}
					break;
				case	4:
					{
						if(btFabs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
							m_simplex->c[1]->w-m_simplex->c[3]->w,
							m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
							return(true);
					}
					break;
				}
				return(false);
			}
			/* Internals	*/ 
			void				getsupport(const btVector3& d,sSV& sv) const
			{
				sv.d	=	d/d.length();
				sv.w	=	m_shape.Support(sv.d);
			}
			void				removevertice(sSimplex& simplex)
			{
				m_free[m_nfree++]=simplex.c[--simplex.rank];
			}
			void				appendvertice(sSimplex& simplex,const btVector3& v)
			{
				simplex.p[simplex.rank]=0;
				simplex.c[simplex.rank]=m_free[--m_nfree];
				getsupport(v,*simplex.c[simplex.rank++]);
			}
			static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
			{
				return(	a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
					a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
					a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
			}
			static btScalar		projectorigin(	const btVector3& a,
				const btVector3& b,
				btScalar* w,U& m)
			{
				const btVector3	d=b-a;
				const btScalar	l=d.length2();
				if(l>GJK_SIMPLEX2_EPS)
				{
					const btScalar	t(l>0?-btDot(a,d)/l:0);
					if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length2()); }
					else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length2()); }
					else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
				}
				return(-1);
			}
			static btScalar		projectorigin(	const btVector3& a,
				const btVector3& b,
				const btVector3& c,
				btScalar* w,U& m)
			{
				static const U		imd3[]={1,2,0};
				const btVector3*	vt[]={&a,&b,&c};
				const btVector3		dl[]={a-b,b-c,c-a};
				const btVector3		n=btCross(dl[0],dl[1]);
				const btScalar		l=n.length2();
				if(l>GJK_SIMPLEX3_EPS)
				{
					btScalar	mindist=-1;
					btScalar	subw[2]={0.f,0.f};
					U			subm(0);
					for(U i=0;i<3;++i)
					{
						if(btDot(*vt[i],btCross(dl[i],n))>0)
						{
							const U			j=imd3[i];
							const btScalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
							if((mindist<0)||(subd<mindist))
							{
								mindist		=	subd;
								m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
								w[i]		=	subw[0];
								w[j]		=	subw[1];
								w[imd3[j]]	=	0;				
							}
						}
					}
					if(mindist<0)
					{
						const btScalar	d=btDot(a,n);	
						const btScalar	s=btSqrt(l);
						const btVector3	p=n*(d/l);
						mindist	=	p.length2();
						m		=	7;
						w[0]	=	(btCross(dl[1],b-p)).length()/s;
						w[1]	=	(btCross(dl[2],c-p)).length()/s;
						w[2]	=	1-(w[0]+w[1]);
					}
					return(mindist);
				}
				return(-1);
			}
			static btScalar		projectorigin(	const btVector3& a,
				const btVector3& b,
				const btVector3& c,
				const btVector3& d,
				btScalar* w,U& m)
			{
				static const U		imd3[]={1,2,0};
				const btVector3*	vt[]={&a,&b,&c,&d};
				const btVector3		dl[]={a-d,b-d,c-d};
				const btScalar		vl=det(dl[0],dl[1],dl[2]);
				const bool			ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
				if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
				{
					btScalar	mindist=-1;
					btScalar	subw[3]={0.f,0.f,0.f};
					U			subm(0);
					for(U i=0;i<3;++i)
					{
						const U			j=imd3[i];
						const btScalar	s=vl*btDot(d,btCross(dl[i],dl[j]));
						if(s>0)
						{
							const btScalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
							if((mindist<0)||(subd<mindist))
							{
								mindist		=	subd;
								m			=	static_cast<U>((subm&1?1<<i:0)+
									(subm&2?1<<j:0)+
									(subm&4?8:0));
								w[i]		=	subw[0];
								w[j]		=	subw[1];
								w[imd3[j]]	=	0;
								w[3]		=	subw[2];
							}
						}
					}
					if(mindist<0)
					{
						mindist	=	0;
						m		=	15;
						w[0]	=	det(c,b,d)/vl;
						w[1]	=	det(a,c,d)/vl;
						w[2]	=	det(b,a,d)/vl;
						w[3]	=	1-(w[0]+w[1]+w[2]);
					}
					return(mindist);
				}
				return(-1);
			}
	};

	// EPA
	struct	EPA
	{
		/* Types		*/ 
		typedef	GJK::sSV	sSV;
		struct	sFace
		{
			btVector3	n;
			btScalar	d;
			sSV*		c[3];
			sFace*		f[3];
			sFace*		l[2];
			U1			e[3];
			U1			pass;
		};
		struct	sList
		{
			sFace*		root;
			U			count;
			sList() : root(0),count(0)	{}
		};
		struct	sHorizon
		{
			sFace*		cf;
			sFace*		ff;
			U			nf;
			sHorizon() : cf(0),ff(0),nf(0)	{}
		};
		struct	eStatus { enum _ {
			Valid,
			Touching,
			Degenerated,
			NonConvex,
			InvalidHull,		
			OutOfFaces,
			OutOfVertices,
			AccuraryReached,
			FallBack,
			Failed		};};
			/* Fields		*/ 
			eStatus::_		m_status;
			GJK::sSimplex	m_result;
			btVector3		m_normal;
			btScalar		m_depth;
			sSV				m_sv_store[EPA_MAX_VERTICES];
			sFace			m_fc_store[EPA_MAX_FACES];
			U				m_nextsv;
			sList			m_hull;
			sList			m_stock;
			/* Methods		*/ 
			EPA()
			{
				Initialize();	
			}


			static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
			{
				fa->e[ea]=(U1)eb;fa->f[ea]=fb;
				fb->e[eb]=(U1)ea;fb->f[eb]=fa;
			}
			static inline void		append(sList& list,sFace* face)
			{
				face->l[0]	=	0;
				face->l[1]	=	list.root;
				if(list.root) list.root->l[0]=face;
				list.root	=	face;
				++list.count;
			}
			static inline void		remove(sList& list,sFace* face)
			{
				if(face->l[1]) face->l[1]->l[0]=face->l[0];
				if(face->l[0]) face->l[0]->l[1]=face->l[1];
				if(face==list.root) list.root=face->l[1];
				--list.count;
			}


			void				Initialize()
			{
				m_status	=	eStatus::Failed;
				m_normal	=	btVector3(0,0,0);
				m_depth		=	0;
				m_nextsv	=	0;
				for(U i=0;i<EPA_MAX_FACES;++i)
				{
					append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
				}
			}
			eStatus::_			Evaluate(GJK& gjk,const btVector3& guess)
			{
				GJK::sSimplex&	simplex=*gjk.m_simplex;
				if((simplex.rank>1)&&gjk.EncloseOrigin())
				{

					/* Clean up				*/ 
					while(m_hull.root)
					{
						sFace*	f = m_hull.root;
						remove(m_hull,f);
						append(m_stock,f);
					}
					m_status	=	eStatus::Valid;
					m_nextsv	=	0;
					/* Orient simplex		*/ 
					if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
						simplex.c[1]->w-simplex.c[3]->w,
						simplex.c[2]->w-simplex.c[3]->w)<0)
					{
						btSwap(simplex.c[0],simplex.c[1]);
						btSwap(simplex.p[0],simplex.p[1]);
					}
					/* Build initial hull	*/ 
					sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
					if(m_hull.count==4)
					{
						sFace*		best=findbest();
						sFace		outer=*best;
						U			pass=0;
						U			iterations=0;
						bind(tetra[0],0,tetra[1],0);
						bind(tetra[0],1,tetra[2],0);
						bind(tetra[0],2,tetra[3],0);
						bind(tetra[1],1,tetra[3],2);
						bind(tetra[1],2,tetra[2],1);
						bind(tetra[2],2,tetra[3],1);
						m_status=eStatus::Valid;
						for(;iterations<EPA_MAX_ITERATIONS;++iterations)
						{
							if(m_nextsv<EPA_MAX_VERTICES)
							{	
								sHorizon		horizon;
								sSV*			w=&m_sv_store[m_nextsv++];
								bool			valid=true;					
								best->pass	=	(U1)(++pass);
								gjk.getsupport(best->n,*w);
								const btScalar	wdist=btDot(best->n,w->w)-best->d;
								if(wdist>EPA_ACCURACY)
								{
									for(U j=0;(j<3)&&valid;++j)
									{
										valid&=expand(	pass,w,
											best->f[j],best->e[j],
											horizon);
									}
									if(valid&&(horizon.nf>=3))
									{
										bind(horizon.cf,1,horizon.ff,2);
										remove(m_hull,best);
										append(m_stock,best);
										best=findbest();
										outer=*best;
									} else { m_status=eStatus::InvalidHull;break; }
								} else { m_status=eStatus::AccuraryReached;break; }
							} else { m_status=eStatus::OutOfVertices;break; }
						}
						const btVector3	projection=outer.n*outer.d;
						m_normal	=	outer.n;
						m_depth		=	outer.d;
						m_result.rank	=	3;
						m_result.c[0]	=	outer.c[0];
						m_result.c[1]	=	outer.c[1];
						m_result.c[2]	=	outer.c[2];
						m_result.p[0]	=	btCross(	outer.c[1]->w-projection,
							outer.c[2]->w-projection).length();
						m_result.p[1]	=	btCross(	outer.c[2]->w-projection,
							outer.c[0]->w-projection).length();
						m_result.p[2]	=	btCross(	outer.c[0]->w-projection,
							outer.c[1]->w-projection).length();
						const btScalar	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
						m_result.p[0]	/=	sum;
						m_result.p[1]	/=	sum;
						m_result.p[2]	/=	sum;
						return(m_status);
					}
				}
				/* Fallback		*/ 
				m_status	=	eStatus::FallBack;
				m_normal	=	-guess;
				const btScalar	nl=m_normal.length();
				if(nl>0)
					m_normal	=	m_normal/nl;
				else
					m_normal	=	btVector3(1,0,0);
				m_depth	=	0;
				m_result.rank=1;
				m_result.c[0]=simplex.c[0];
				m_result.p[0]=1;	
				return(m_status);
			}
			bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
			{
				const btVector3 ba = b->w - a->w;
				const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
				const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required

				if(a_dot_nab < 0)
				{
					// Outside of edge a->b

					const btScalar ba_l2 = ba.length2();
					const btScalar a_dot_ba = btDot(a->w, ba);
					const btScalar b_dot_ba = btDot(b->w, ba);

					if(a_dot_ba > 0)
					{
						// Pick distance vertex a
						dist = a->w.length();
					}
					else if(b_dot_ba < 0)
					{
						// Pick distance vertex b
						dist = b->w.length();
					}
					else
					{
						// Pick distance to edge a->b
						const btScalar a_dot_b = btDot(a->w, b->w);
						dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
					}

					return true;
				}

				return false;
			}
			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
			{
				if(m_stock.root)
				{
					sFace*	face=m_stock.root;
					remove(m_stock,face);
					append(m_hull,face);
					face->pass	=	0;
					face->c[0]	=	a;
					face->c[1]	=	b;
					face->c[2]	=	c;
					face->n		=	btCross(b->w-a->w,c->w-a->w);
					const btScalar	l=face->n.length();
					const bool		v=l>EPA_ACCURACY;

					if(v)
					{
						if(!(getedgedist(face, a, b, face->d) ||
							 getedgedist(face, b, c, face->d) ||
							 getedgedist(face, c, a, face->d)))
						{
							// Origin projects to the interior of the triangle
							// Use distance to triangle plane
							face->d = btDot(a->w, face->n) / l;
						}

						face->n /= l;
						if(forced || (face->d >= -EPA_PLANE_EPS))
						{
							return face;
						}
						else
							m_status=eStatus::NonConvex;
					}
					else
						m_status=eStatus::Degenerated;

					remove(m_hull, face);
					append(m_stock, face);
					return 0;

				}
				m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
				return 0;
			}
			sFace*				findbest()
			{
				sFace*		minf=m_hull.root;
				btScalar	mind=minf->d*minf->d;
				for(sFace* f=minf->l[1];f;f=f->l[1])
				{
					const btScalar	sqd=f->d*f->d;
					if(sqd<mind)
					{
						minf=f;
						mind=sqd;
					}
				}
				return(minf);
			}
			bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
			{
				static const U	i1m3[]={1,2,0};
				static const U	i2m3[]={2,0,1};
				if(f->pass!=pass)
				{
					const U	e1=i1m3[e];
					if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
					{
						sFace*	nf=newface(f->c[e1],f->c[e],w,false);
						if(nf)
						{
							bind(nf,0,f,e);
							if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
							horizon.cf=nf;
							++horizon.nf;
							return(true);
						}
					}
					else
					{
						const U	e2=i2m3[e];
						f->pass		=	(U1)pass;
						if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
							expand(pass,w,f->f[e2],f->e[e2],horizon))
						{
							remove(m_hull,f);
							append(m_stock,f);
							return(true);
						}
					}
				}
				return(false);
			}

	};

	//
	static void	Initialize(	const btConvexShape* shape0,const btTransform& wtrs0,
		const btConvexShape* shape1,const btTransform& wtrs1,
		btGjkEpaSolver2::sResults& results,
		tShape& shape,
		bool withmargins)
	{
		/* Results		*/ 
		results.witnesses[0]	=
			results.witnesses[1]	=	btVector3(0,0,0);
		results.status			=	btGjkEpaSolver2::sResults::Separated;
		/* Shape		*/ 
		shape.m_shapes[0]		=	shape0;
		shape.m_shapes[1]		=	shape1;
		shape.m_toshape1		=	wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
		shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
		shape.EnableMargin(withmargins);
	}

}

//
// Api
//

using namespace	gjkepa2_impl;

//
int			btGjkEpaSolver2::StackSizeRequirement()
{
	return(sizeof(GJK)+sizeof(EPA));
}

//
bool		btGjkEpaSolver2::Distance(	const btConvexShape*	shape0,
									  const btTransform&		wtrs0,
									  const btConvexShape*	shape1,
									  const btTransform&		wtrs1,
									  const btVector3&		guess,
									  sResults&				results)
{
	tShape			shape;
	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
	GJK				gjk;
	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
	if(gjk_status==GJK::eStatus::Valid)
	{
		btVector3	w0=btVector3(0,0,0);
		btVector3	w1=btVector3(0,0,0);
		for(U i=0;i<gjk.m_simplex->rank;++i)
		{
			const btScalar	p=gjk.m_simplex->p[i];
			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
		}
		results.witnesses[0]	=	wtrs0*w0;
		results.witnesses[1]	=	wtrs0*w1;
		results.normal			=	w0-w1;
		results.distance		=	results.normal.length();
		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
		return(true);
	}
	else
	{
		results.status	=	gjk_status==GJK::eStatus::Inside?
			sResults::Penetrating	:
		sResults::GJK_Failed	;
		return(false);
	}
}

//
bool	btGjkEpaSolver2::Penetration(	const btConvexShape*	shape0,
									 const btTransform&		wtrs0,
									 const btConvexShape*	shape1,
									 const btTransform&		wtrs1,
									 const btVector3&		guess,
									 sResults&				results,
									 bool					usemargins)
{
	tShape			shape;
	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
	GJK				gjk;	
	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
	switch(gjk_status)
	{
	case	GJK::eStatus::Inside:
		{
			EPA				epa;
			EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
			if(epa_status!=EPA::eStatus::Failed)
			{
				btVector3	w0=btVector3(0,0,0);
				for(U i=0;i<epa.m_result.rank;++i)
				{
					w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
				}
				results.status			=	sResults::Penetrating;
				results.witnesses[0]	=	wtrs0*w0;
				results.witnesses[1]	=	wtrs0*(w0-epa.m_normal*epa.m_depth);
				results.normal			=	-epa.m_normal;
				results.distance		=	-epa.m_depth;
				return(true);
			} else results.status=sResults::EPA_Failed;
		}
		break;
	case	GJK::eStatus::Failed:
		results.status=sResults::GJK_Failed;
		break;
		default:
					{
					}
	}
	return(false);
}

#ifndef __SPU__
//
btScalar	btGjkEpaSolver2::SignedDistance(const btVector3& position,
											btScalar margin,
											const btConvexShape* shape0,
											const btTransform& wtrs0,
											sResults& results)
{
	tShape			shape;
	btSphereShape	shape1(margin);
	btTransform		wtrs1(btQuaternion(0,0,0,1),position);
	Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
	GJK				gjk;	
	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
	if(gjk_status==GJK::eStatus::Valid)
	{
		btVector3	w0=btVector3(0,0,0);
		btVector3	w1=btVector3(0,0,0);
		for(U i=0;i<gjk.m_simplex->rank;++i)
		{
			const btScalar	p=gjk.m_simplex->p[i];
			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
		}
		results.witnesses[0]	=	wtrs0*w0;
		results.witnesses[1]	=	wtrs0*w1;
		const btVector3	delta=	results.witnesses[1]-
			results.witnesses[0];
		const btScalar	margin=	shape0->getMarginNonVirtual()+
			shape1.getMarginNonVirtual();
		const btScalar	length=	delta.length();	
		results.normal			=	delta/length;
		results.witnesses[0]	+=	results.normal*margin;
		return(length-margin);
	}
	else
	{
		if(gjk_status==GJK::eStatus::Inside)
		{
			if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
			{
				const btVector3	delta=	results.witnesses[0]-
					results.witnesses[1];
				const btScalar	length=	delta.length();
				if (length >= SIMD_EPSILON)
					results.normal	=	delta/length;			
				return(-length);
			}
		}	
	}
	return(SIMD_INFINITY);
}

//
bool	btGjkEpaSolver2::SignedDistance(const btConvexShape*	shape0,
										const btTransform&		wtrs0,
										const btConvexShape*	shape1,
										const btTransform&		wtrs1,
										const btVector3&		guess,
										sResults&				results)
{
	if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
		return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
	else
		return(true);
}
#endif //__SPU__

/* Symbols cleanup		*/ 

#undef GJK_MAX_ITERATIONS
#undef GJK_ACCURACY
#undef GJK_MIN_DISTANCE
#undef GJK_DUPLICATED_EPS
#undef GJK_SIMPLEX2_EPS
#undef GJK_SIMPLEX3_EPS
#undef GJK_SIMPLEX4_EPS

#undef EPA_MAX_VERTICES
#undef EPA_MAX_FACES
#undef EPA_MAX_ITERATIONS
#undef EPA_ACCURACY
#undef EPA_FALLBACK
#undef EPA_PLANE_EPS
#undef EPA_INSIDE_EPS