/*
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 "b3GjkEpa.h"

#include "b3SupportMappings.h"

namespace gjkepa2_impl2
{

	// Config

	/* GJK	*/ 
#define GJK_MAX_ITERATIONS	128
#define GJK_ACCURACY		((b3Scalar)0.0001)
#define GJK_MIN_DISTANCE	((b3Scalar)0.0001)
#define GJK_DUPLICATED_EPS	((b3Scalar)0.0001)
#define GJK_SIMPLEX2_EPS	((b3Scalar)0.0)
#define GJK_SIMPLEX3_EPS	((b3Scalar)0.0)
#define GJK_SIMPLEX4_EPS	((b3Scalar)0.0)

	/* EPA	*/ 
#define EPA_MAX_VERTICES	64
#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
#define EPA_MAX_ITERATIONS	255
#define EPA_ACCURACY		((b3Scalar)0.0001)
#define EPA_FALLBACK		(10*EPA_ACCURACY)
#define EPA_PLANE_EPS		((b3Scalar)0.00001)
#define EPA_INSIDE_EPS		((b3Scalar)0.01)


	// Shorthands
	

	// MinkowskiDiff
	struct	b3MinkowskiDiff
	{
		

		const b3ConvexPolyhedronData*	m_shapes[2];
		

		b3Matrix3x3				m_toshape1;
		b3Transform				m_toshape0;

		bool					m_enableMargin;
		

			void					EnableMargin(bool enable)
		{
			m_enableMargin = enable;
		}	
		inline b3Vector3		Support0(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA) const
		{
			if (m_enableMargin)
			{
				return localGetSupportVertexWithMargin(d,m_shapes[0],verticesA,0.f);
			} else
			{
				return localGetSupportVertexWithoutMargin(d,m_shapes[0],verticesA);
			}
		}
		inline b3Vector3		Support1(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesB) const
		{
			if (m_enableMargin)
			{
				return m_toshape0*(localGetSupportVertexWithMargin(m_toshape1*d,m_shapes[1],verticesB,0.f));
			} else
			{
				return m_toshape0*(localGetSupportVertexWithoutMargin(m_toshape1*d,m_shapes[1],verticesB));
			}
		}

		inline b3Vector3		Support(const b3Vector3& d,  const b3AlignedObjectArray<b3Vector3>& verticesA,  const b3AlignedObjectArray<b3Vector3>& verticesB) const
		{
			return(Support0(d,verticesA)-Support1(-d,verticesB));
		}
		b3Vector3				Support(const b3Vector3& d,unsigned int index,const b3AlignedObjectArray<b3Vector3>& verticesA,  const b3AlignedObjectArray<b3Vector3>& verticesB) const
		{
			if(index)
				return(Support1(d,verticesA));
			else
				return(Support0(d,verticesB));
		}
	};

	typedef	b3MinkowskiDiff	tShape;


	// GJK
	struct	b3GJK
	{
		/* Types		*/ 
		struct	sSV
		{
			b3Vector3	d,w;
		};
		struct	sSimplex
		{
			sSV*		c[4];
			b3Scalar	p[4];
			unsigned int			rank;
		};
		struct	eStatus	{ enum _ {
			Valid,
			Inside,
			Failed		};};
			/* Fields		*/ 
			tShape			m_shape;
			const b3AlignedObjectArray<b3Vector3>& m_verticesA;
			const b3AlignedObjectArray<b3Vector3>& m_verticesB;
			b3Vector3		m_ray;
			b3Scalar		m_distance;
			sSimplex		m_simplices[2];
			sSV				m_store[4];
			sSV*			m_free[4];
			unsigned int				m_nfree;
			unsigned int				m_current;
			sSimplex*		m_simplex;
			eStatus::_		m_status;
			/* Methods		*/ 
			b3GJK(const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB)
				:m_verticesA(verticesA),m_verticesB(verticesB)
			{
				Initialize();
			}
			void				Initialize()
			{
				m_ray		=	b3MakeVector3(0,0,0);
				m_nfree		=	0;
				m_status	=	eStatus::Failed;
				m_current	=	0;
				m_distance	=	0;
			}
			eStatus::_			Evaluate(const tShape& shapearg,const b3Vector3& guess)
			{
				unsigned int			iterations=0;
				b3Scalar	sqdist=0;
				b3Scalar	alpha=0;
				b3Vector3	lastw[4];
				unsigned int			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 b3Scalar	sqrl=	m_ray.length2();
				appendvertice(m_simplices[0],sqrl>0?-m_ray:b3MakeVector3(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 unsigned int		next=1-m_current;
					sSimplex&	cs=m_simplices[m_current];
					sSimplex&	ns=m_simplices[next];
					/* Check zero							*/ 
					const b3Scalar	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 b3Vector3&	w=cs.c[cs.rank-1]->w;
					bool				found=false;
					for(unsigned int 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 b3Scalar	omega=b3Dot(m_ray,w)/rl;
					alpha=b3Max(omega,alpha);
					if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
					{/* Return old simplex				*/ 
						removevertice(m_simplices[m_current]);
						break;
					}		
					/* Reduce simplex						*/ 
					b3Scalar	weights[4];
					unsigned int			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		=	b3MakeVector3(0,0,0);
						m_current	=	next;
						for(unsigned int 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(unsigned int i=0;i<3;++i)
						{
							b3Vector3		axis=b3MakeVector3(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 b3Vector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
						for(unsigned int i=0;i<3;++i)
						{
							b3Vector3		axis=b3MakeVector3(0,0,0);
							axis[i]=1;
							const b3Vector3	p=b3Cross(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 b3Vector3	n=b3Cross(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(b3Fabs(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 b3Vector3& d,sSV& sv) const
			{
				sv.d	=	d/d.length();
				sv.w	=	m_shape.Support(sv.d,m_verticesA,m_verticesB);
			}
			void				removevertice(sSimplex& simplex)
			{
				m_free[m_nfree++]=simplex.c[--simplex.rank];
			}
			void				appendvertice(sSimplex& simplex,const b3Vector3& v)
			{
				simplex.p[simplex.rank]=0;
				simplex.c[simplex.rank]=m_free[--m_nfree];
				getsupport(v,*simplex.c[simplex.rank++]);
			}
			static b3Scalar		det(const b3Vector3& a,const b3Vector3& b,const b3Vector3& 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 b3Scalar		projectorigin(	const b3Vector3& a,
				const b3Vector3& b,
				b3Scalar* w,unsigned int& m)
			{
				const b3Vector3	d=b-a;
				const b3Scalar	l=d.length2();
				if(l>GJK_SIMPLEX2_EPS)
				{
					const b3Scalar	t(l>0?-b3Dot(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 b3Scalar		projectorigin(	const b3Vector3& a,
				const b3Vector3& b,
				const b3Vector3& c,
				b3Scalar* w,unsigned int& m)
			{
				static const unsigned int		imd3[]={1,2,0};
				const b3Vector3*	vt[]={&a,&b,&c};
				const b3Vector3		dl[]={a-b,b-c,c-a};
				const b3Vector3		n=b3Cross(dl[0],dl[1]);
				const b3Scalar		l=n.length2();
				if(l>GJK_SIMPLEX3_EPS)
				{
					b3Scalar	mindist=-1;
					b3Scalar	subw[2]={0.f,0.f};
					unsigned int			subm(0);
					for(unsigned int i=0;i<3;++i)
					{
						if(b3Dot(*vt[i],b3Cross(dl[i],n))>0)
						{
							const unsigned int			j=imd3[i];
							const b3Scalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
							if((mindist<0)||(subd<mindist))
							{
								mindist		=	subd;
								m			=	static_cast<unsigned int>(((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 b3Scalar	d=b3Dot(a,n);	
						const b3Scalar	s=b3Sqrt(l);
						const b3Vector3	p=n*(d/l);
						mindist	=	p.length2();
						m		=	7;
						w[0]	=	(b3Cross(dl[1],b-p)).length()/s;
						w[1]	=	(b3Cross(dl[2],c-p)).length()/s;
						w[2]	=	1-(w[0]+w[1]);
					}
					return(mindist);
				}
				return(-1);
			}
			static b3Scalar		projectorigin(	const b3Vector3& a,
				const b3Vector3& b,
				const b3Vector3& c,
				const b3Vector3& d,
				b3Scalar* w,unsigned int& m)
			{
				static const unsigned int		imd3[]={1,2,0};
				const b3Vector3*	vt[]={&a,&b,&c,&d};
				const b3Vector3		dl[]={a-d,b-d,c-d};
				const b3Scalar		vl=det(dl[0],dl[1],dl[2]);
				const bool			ng=(vl*b3Dot(a,b3Cross(b-c,a-b)))<=0;
				if(ng&&(b3Fabs(vl)>GJK_SIMPLEX4_EPS))
				{
					b3Scalar	mindist=-1;
					b3Scalar	subw[3]={0.f,0.f,0.f};
					unsigned int			subm(0);
					for(unsigned int i=0;i<3;++i)
					{
						const unsigned int			j=imd3[i];
						const b3Scalar	s=vl*b3Dot(d,b3Cross(dl[i],dl[j]));
						if(s>0)
						{
							const b3Scalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
							if((mindist<0)||(subd<mindist))
							{
								mindist		=	subd;
								m			=	static_cast<unsigned int>((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	b3EPA
	{
		/* Types		*/ 
		typedef	b3GJK::sSV	sSV;
		struct	sFace
		{
			b3Vector3	n;
			b3Scalar	d;
			sSV*		c[3];
			sFace*		f[3];
			sFace*		l[2];
			unsigned char			e[3];
			unsigned char			pass;
		};
		struct	sList
		{
			sFace*		root;
			unsigned int			count;
			sList() : root(0),count(0)	{}
		};
		struct	sHorizon
		{
			sFace*		cf;
			sFace*		ff;
			unsigned int			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;
			b3GJK::sSimplex	m_result;
			b3Vector3		m_normal;
			b3Scalar		m_depth;
			sSV				m_sv_store[EPA_MAX_VERTICES];
			sFace			m_fc_store[EPA_MAX_FACES];
			unsigned int				m_nextsv;
			sList			m_hull;
			sList			m_stock;
			/* Methods		*/ 
			b3EPA()
			{
				Initialize();	
			}


			static inline void		bind(sFace* fa,unsigned int ea,sFace* fb,unsigned int eb)
			{
				fa->e[ea]=(unsigned char)eb;fa->f[ea]=fb;
				fb->e[eb]=(unsigned char)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	=	b3MakeVector3(0,0,0);
				m_depth		=	0;
				m_nextsv	=	0;
				for(unsigned int i=0;i<EPA_MAX_FACES;++i)
				{
					append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
				}
			}
			eStatus::_			Evaluate(b3GJK& gjk,const b3Vector3& guess)
			{
				b3GJK::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)
					{
						b3Swap(simplex.c[0],simplex.c[1]);
						b3Swap(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;
						unsigned int			pass=0;
						unsigned int			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	=	(unsigned char)(++pass);
								gjk.getsupport(best->n,*w);
								const b3Scalar	wdist=b3Dot(best->n,w->w)-best->d;
								if(wdist>EPA_ACCURACY)
								{
									for(unsigned int 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::Failed;
										//m_status=eStatus::InvalidHull;
									break; }
								} else { m_status=eStatus::AccuraryReached;break; }
							} else { m_status=eStatus::OutOfVertices;break; }
						}
						const b3Vector3	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]	=	b3Cross(	outer.c[1]->w-projection,
							outer.c[2]->w-projection).length();
						m_result.p[1]	=	b3Cross(	outer.c[2]->w-projection,
							outer.c[0]->w-projection).length();
						m_result.p[2]	=	b3Cross(	outer.c[0]->w-projection,
							outer.c[1]->w-projection).length();
						const b3Scalar	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 b3Scalar	nl=m_normal.length();
				if(nl>0)
					m_normal	=	m_normal/nl;
				else
					m_normal	=	b3MakeVector3(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, b3Scalar& dist)
			{
				const b3Vector3 ba = b->w - a->w;
				const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane
				const b3Scalar a_dot_nab = b3Dot(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 b3Scalar ba_l2 = ba.length2();
					const b3Scalar a_dot_ba = b3Dot(a->w, ba);
					const b3Scalar b_dot_ba = b3Dot(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 b3Scalar a_dot_b = b3Dot(a->w, b->w);
						dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)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		=	b3Cross(b->w-a->w,c->w-a->w);
					const b3Scalar	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 = b3Dot(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;
				b3Scalar	mind=minf->d*minf->d;
				for(sFace* f=minf->l[1];f;f=f->l[1])
				{
					const b3Scalar	sqd=f->d*f->d;
					if(sqd<mind)
					{
						minf=f;
						mind=sqd;
					}
				}
				return(minf);
			}
			bool				expand(unsigned int pass,sSV* w,sFace* f,unsigned int e,sHorizon& horizon)
			{
				static const unsigned int	i1m3[]={1,2,0};
				static const unsigned int	i2m3[]={2,0,1};
				if(f->pass!=pass)
				{
					const unsigned int	e1=i1m3[e];
					if((b3Dot(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 unsigned int	e2=i2m3[e];
						f->pass		=	(unsigned char)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 b3Transform&	transA, const b3Transform&	transB,
								const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, 
								const b3AlignedObjectArray<b3Vector3>& verticesA,
								const b3AlignedObjectArray<b3Vector3>& verticesB,
		b3GjkEpaSolver2::sResults& results,
		tShape& shape,
		bool withmargins)
	{
		/* Results		*/ 
		results.witnesses[0]	=
			results.witnesses[1]	=	b3MakeVector3(0,0,0);
		results.status			=	b3GjkEpaSolver2::sResults::Separated;
		/* Shape		*/ 
		shape.m_shapes[0]		=	hullA;
		shape.m_shapes[1]		=	hullB;
		shape.m_toshape1		=	transB.getBasis().transposeTimes(transA.getBasis());
		shape.m_toshape0		=	transA.inverseTimes(transB);
		shape.EnableMargin(withmargins);
	}

}

//
// Api
//

using namespace	gjkepa2_impl2;

//
int			b3GjkEpaSolver2::StackSizeRequirement()
{
	return(sizeof(b3GJK)+sizeof(b3EPA));
}

//
bool		b3GjkEpaSolver2::Distance(	const b3Transform&	transA, const b3Transform&	transB,
										const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, 
										const b3AlignedObjectArray<b3Vector3>& verticesA,
										const b3AlignedObjectArray<b3Vector3>& verticesB,
									  const b3Vector3&		guess,
									  sResults&				results)
{
	tShape			shape;
	Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,false);
	b3GJK				gjk(verticesA,verticesB);
	b3GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
	if(gjk_status==b3GJK::eStatus::Valid)
	{
		b3Vector3	w0=b3MakeVector3(0,0,0);
		b3Vector3	w1=b3MakeVector3(0,0,0);
		for(unsigned int i=0;i<gjk.m_simplex->rank;++i)
		{
			const b3Scalar	p=gjk.m_simplex->p[i];
			w0+=shape.Support( gjk.m_simplex->c[i]->d,0,verticesA,verticesB)*p;
			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1,verticesA,verticesB)*p;
		}
		results.witnesses[0]	=	transA*w0;
		results.witnesses[1]	=	transA*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==b3GJK::eStatus::Inside?
			sResults::Penetrating	:
		sResults::GJK_Failed	;
		return(false);
	}
}

//
bool	b3GjkEpaSolver2::Penetration(	const b3Transform&	transA, const b3Transform&	transB,
										const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, 
										const b3AlignedObjectArray<b3Vector3>& verticesA,
										const b3AlignedObjectArray<b3Vector3>& verticesB,
									 const b3Vector3&		guess,
									 sResults&				results,
									 bool					usemargins)
{

	tShape			shape;
	Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,usemargins);
	b3GJK				gjk(verticesA,verticesB);
	b3GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
	switch(gjk_status)
	{
	case	b3GJK::eStatus::Inside:
		{
			b3EPA				epa;
			b3EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
			if(epa_status!=b3EPA::eStatus::Failed)
			{
				b3Vector3	w0=b3MakeVector3(0,0,0);
				for(unsigned int i=0;i<epa.m_result.rank;++i)
				{
					w0+=shape.Support(epa.m_result.c[i]->d,0,verticesA,verticesB)*epa.m_result.p[i];
				}
				results.status			=	sResults::Penetrating;
				results.witnesses[0]	=	transA*w0;
				results.witnesses[1]	=	transA*(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	b3GJK::eStatus::Failed:
		results.status=sResults::GJK_Failed;
		break;
		default:
					{
					}
	}
	return(false);
}


#if 0
//
b3Scalar	b3GjkEpaSolver2::SignedDistance(const b3Vector3& position,
											b3Scalar margin,
											const b3Transform&	transA,
											const b3ConvexPolyhedronData& hullA, 
											const b3AlignedObjectArray<b3Vector3>& verticesA,
											sResults& results)
{
	tShape			shape;
	btSphereShape	shape1(margin);
	b3Transform		wtrs1(b3Quaternion(0,0,0,1),position);
	Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
	GJK				gjk;	
	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,b3Vector3(1,1,1));
	if(gjk_status==GJK::eStatus::Valid)
	{
		b3Vector3	w0=b3Vector3(0,0,0);
		b3Vector3	w1=b3Vector3(0,0,0);
		for(unsigned int i=0;i<gjk.m_simplex->rank;++i)
		{
			const b3Scalar	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 b3Vector3	delta=	results.witnesses[1]-
			results.witnesses[0];
		const b3Scalar	margin=	shape0->getMarginNonVirtual()+
			shape1.getMarginNonVirtual();
		const b3Scalar	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 b3Vector3	delta=	results.witnesses[0]-
					results.witnesses[1];
				const b3Scalar	length=	delta.length();
				if (length >= B3_EPSILON)
					results.normal	=	delta/length;			
				return(-length);
			}
		}	
	}
	return(B3_INFINITY);
}

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


/* 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