path: root/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.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.
*/

/*
GJK-EPA collision solver by Nathanael Presson
Nov.2006
*/

#include "btGjkEpa.h"
#include <string.h> //for memset
#include "LinearMath/btStackAlloc.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 gjkepa_impl
{

//
// Port. typedefs
//

typedef	btScalar		F;
typedef bool			Z;
typedef int				I;
typedef unsigned int	U;
typedef unsigned char	U1;
typedef unsigned short	U2;

typedef btVector3		Vector3;
typedef btMatrix3x3		Rotation;

//
// Config
//

#if 0
#define BTLOCALSUPPORT	localGetSupportingVertexWithoutMargin
#else
#define BTLOCALSUPPORT	localGetSupportingVertex
#endif

//
// Const
//


#define cstInf				SIMD_INFINITY
#define cstPi				SIMD_PI
#define	cst2Pi				SIMD_2_PI
#define GJK_maxiterations	(128)
#define GJK_hashsize		(1<<6)
#define GJK_hashmask		(GJK_hashsize-1)
#define GJK_insimplex_eps	F(0.0001)
#define GJK_sqinsimplex_eps	(GJK_insimplex_eps*GJK_insimplex_eps)
#define EPA_maxiterations	256
#define	EPA_inface_eps		F(0.01)
#define EPA_accuracy		F(0.001)

//
// Utils
//

static inline F								Abs(F v)					{ return(v<0?-v:v); }
static inline F								Sign(F v)					{ return(F(v<0?-1:1)); }
template <typename T> static inline void	Swap(T& a,T& b)				{ T 
t(a);a=b;b=t; }
template <typename T> static inline T		Min(const T& a,const T& b)	{ 
return(a<b?a:b); }
template <typename T> static inline T		Max(const T& a,const T& b)	{ 
return(a>b?a:b); }
static inline void							ClearMemory(void* p,U sz)	{ memset(p,0,(size_t)sz); 
}
#if 0
template <typename T> static inline void	Raise(const T& object)		{
throw(object); }
#else
template <typename T> static inline void	Raise(const T&)				{}
#endif



//
// GJK
//
struct	GJK
	{
	struct Mkv
		{
		Vector3	w;		/* Minkowski vertice	*/
		Vector3	r;		/* Ray					*/
		};
	struct He
		{
		Vector3	v;
		He*		n;
		};
	btStackAlloc*				sa;
	btBlock*		sablock;
	He*						table[GJK_hashsize];
	Rotation				wrotations[2];
	Vector3					positions[2];
	const btConvexShape*	shapes[2];
	Mkv						simplex[5];
	Vector3					ray;
	U						order;
	U						iterations;
	F						margin;
	Z						failed;
	//
					GJK(btStackAlloc* psa,
						const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0,
						const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1,
						F pmargin=0)
		{
		wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0;
		wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1;
		sa		=psa;
		sablock	=sa->beginBlock();
		margin	=pmargin;
		failed	=false;
		}
	//
					~GJK()
		{
		sa->endBlock(sablock);
		}
	// vdh : very dumm hash
	static inline U	Hash(const Vector3& v)
		{
		//this doesn't compile under GCC 3.3.5, so add the ()...
		//const U	h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473));
		//return(((*((const U*)&h))*169639)&GJK_hashmask);
	    const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473));
	    return(((*((const U*)&h))*169639)&GJK_hashmask);
		}
	//
	inline Vector3	LocalSupport(const Vector3& d,U i) const
		{
		return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]);
		}
	//
	inline void		Support(const Vector3& d,Mkv& v) const
		{
		v.r	=	d;
		v.w	=	LocalSupport(d,0)-LocalSupport(-d,1)+d*margin;
		}
	#define SPX(_i_)	simplex[_i_]
	#define SPXW(_i_)	simplex[_i_].w
	//
	inline Z		FetchSupport()
		{
		const U			h(Hash(ray));
		He*				e = (He*)(table[h]);
		while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; }
		e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e;
		Support(ray,simplex[++order]);
		return(ray.dot(SPXW(order))>0);
		}
	//
	inline Z		SolveSimplex2(const Vector3& ao,const Vector3& ab)
		{
		if(ab.dot(ao)>=0)
			{
			const Vector3	cabo(cross(ab,ao));
			if(cabo.length2()>GJK_sqinsimplex_eps)
				{ ray=cross(cabo,ab); }
				else
				{ return(true); }
			}
			else
			{ order=0;SPX(0)=SPX(1);ray=ao;	}
		return(false);
		}
	//
	inline Z		SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& 
ac)
		{
		return(SolveSimplex3a(ao,ab,ac,cross(ab,ac)));
		}
	//
	inline Z		SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& 
ac,const Vector3& cabc)
		{
				if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps)
			{ order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab));	}
		else	if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps)
			{ order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); }
		else
			{
			const F			d(cabc.dot(ao));
			if(Abs(d)>GJK_insimplex_eps)
				{
				if(d>0)
					{ ray=cabc; }
					else
					{ ray=-cabc;Swap(SPX(0),SPX(1)); }
				return(false);
				} else return(true);
			}
		}
	//
	inline Z		SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& 
ac,const Vector3& ad)
		{
		Vector3			crs;
				if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps)
			{ 
order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); 
}
		else	if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps)
			{ order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); }
		else	if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps)
			{ 
order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); 
}
		else	return(true);
		}
	//
	inline Z		SearchOrigin(const Vector3& initray=Vector3(1,0,0))
		{
		iterations	=	0;
		order		=	(U)-1;
		failed		=	false;
		ray			=	initray.normalized();
		ClearMemory(table,sizeof(void*)*GJK_hashsize);
		FetchSupport();
		ray			=	-SPXW(0);
		for(;iterations<GJK_maxiterations;++iterations)
			{
			const F		rl(ray.length());
			ray/=rl>0?rl:1;
			if(FetchSupport())
				{
				Z	found(false);
				switch(order)
					{
					case	1:	found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break;
					case	2:	found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break;
					case	3:	found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break;
					}
				if(found) return(true);
				} else return(false);
			}
		failed=true;
		return(false);
		}
	//
	inline Z		EncloseOrigin()
		{
		switch(order)
			{
			/* Point		*/
			case	0:	break;
			/* Line			*/
			case	1:
				{
				const Vector3	ab(SPXW(1)-SPXW(0));
				const Vector3	b[]={	cross(ab,Vector3(1,0,0)),
										cross(ab,Vector3(0,1,0)),
										cross(ab,Vector3(0,0,1))};
				const F			m[]={b[0].length2(),b[1].length2(),b[2].length2()};
				const Rotation	r(btQuaternion(ab.normalized(),cst2Pi/3));
				Vector3			w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]);
				Support(w.normalized(),simplex[4]);w=r*w;
				Support(w.normalized(),simplex[2]);w=r*w;
				Support(w.normalized(),simplex[3]);w=r*w;
				order=4;
				return(true);
				}
			break;
			/* Triangle		*/
			case	2:
				{
				const 
Vector3	n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized());
				Support( n,simplex[3]);
				Support(-n,simplex[4]);
				order=4;
				return(true);
				}
			break;
			/* Tetrahedron	*/
			case	3:	return(true);
			/* Hexahedron	*/
			case	4:	return(true);
			}
		return(false);
		}
	#undef SPX
	#undef SPXW
	};

//
// EPA
//
struct	EPA
	{
	//
	struct Face
		{
		const GJK::Mkv*	v[3];
		Face*			f[3];
		U				e[3];
		Vector3			n;
		F				d;
		U				mark;
		Face*			prev;
		Face*			next;
		Face()			{}
		};
	//
	GJK*			gjk;
	btStackAlloc*		sa;
	Face*			root;
	U				nfaces;
	U				iterations;
	Vector3			features[2][3];
	Vector3			nearest[2];
	Vector3			normal;
	F				depth;
	Z				failed;
	//
					EPA(GJK* pgjk)
		{
		gjk		=	pgjk;
		sa		=	pgjk->sa;
		}
	//
					~EPA()
		{
		}
	//
	inline Vector3	GetCoordinates(const Face* face) const
		{
		const Vector3	o(face->n*-face->d);
		const F			a[]={	cross(face->v[0]->w-o,face->v[1]->w-o).length(),
								cross(face->v[1]->w-o,face->v[2]->w-o).length(),
								cross(face->v[2]->w-o,face->v[0]->w-o).length()};
		const F			sm(a[0]+a[1]+a[2]);
		return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1));
		}
	//
	inline Face*	FindBest() const
		{
		Face*	bf = 0;
		if(root)
			{
			Face*	cf = root;
			F		bd(cstInf);
			do	{
				if(cf->d<bd) { bd=cf->d;bf=cf; }
				} while(0!=(cf=cf->next));
			}
		return(bf);
		}
	//
	inline Z		Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv*
c) const
		{
		const Vector3	nrm(cross(b->w-a->w,c->w-a->w));
		const F			len(nrm.length());
		const Z			valid(	(cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&&
								(cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&&
								(cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps));
		f->v[0]	=	a;
		f->v[1]	=	b;
		f->v[2]	=	c;
		f->mark	=	0;
		f->n	=	nrm/(len>0?len:cstInf);
		f->d	=	Max<F>(0,-f->n.dot(a->w));
		return(valid);
		}
	//
	inline Face*	NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
		{
		Face*	pf = (Face*)sa->allocate(sizeof(Face));
		if(Set(pf,a,b,c))
			{
			if(root) root->prev=pf;
			pf->prev=0;
			pf->next=root;
			root	=pf;
			++nfaces;
			}
			else
			{
			pf->prev=pf->next=0;
			}
		return(pf);
		}
	//
	inline void		Detach(Face* face)
		{
		if(face->prev||face->next)
			{
			--nfaces;
			if(face==root)
				{ root=face->next;root->prev=0; }
				else
				{
				if(face->next==0)
					{ face->prev->next=0; }
					else
					{ face->prev->next=face->next;face->next->prev=face->prev; }
				}
			face->prev=face->next=0;
			}
		}
	//
	inline void		Link(Face* f0,U e0,Face* f1,U e1) const
		{
		f0->f[e0]=f1;f1->e[e1]=e0;
		f1->f[e1]=f0;f0->e[e0]=e1;
		}
	//
	GJK::Mkv*		Support(const Vector3& w) const
		{
		GJK::Mkv*		v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));
		gjk->Support(w,*v);
		return(v);
		}
	//
	U				BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& 
ff)
		{
		static const U	mod3[]={0,1,2,0,1};
		U				ne(0);
		if(f.mark!=markid)
			{
			const U	e1(mod3[e+1]);
			if((f.n.dot(w->w)+f.d)>0)
				{
				Face*	nf = NewFace(f.v[e1],f.v[e],w);
				Link(nf,0,&f,e);
				if(cf) Link(cf,1,nf,2); else ff=nf;
				cf=nf;ne=1;
				}
				else
				{
				const U	e2(mod3[e+2]);
				Detach(&f);
				f.mark	=	markid;
				ne		+=	BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff);
				ne		+=	BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff);
				}
			}
		return(ne);
		}
	//
	inline F		EvaluatePD(F accuracy=EPA_accuracy)
		{
		btBlock*	sablock = sa->beginBlock();
		Face*				bestface = 0;
		U					markid(1);
		depth		=	-cstInf;
		normal		=	Vector3(0,0,0);
		root		=	0;
		nfaces		=	0;
		iterations	=	0;
		failed		=	false;
		/* Prepare hull		*/
		if(gjk->EncloseOrigin())
			{
			const U*		pfidx = 0;
			U				nfidx= 0;
			const U*		peidx = 0;
			U				neidx = 0;
			GJK::Mkv*		basemkv[5];
			Face*			basefaces[6];
			switch(gjk->order)
				{
				/* Tetrahedron		*/
				case	3:	{
							static const U	fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}};
							static const 
U	eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}};
							pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6;
							}	break;
				/* Hexahedron		*/
				case	4:	{
							static const 
U	fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}};
							static const 
U	eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}};
							pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9;
							}	break;
				}
			U i;

			for( i=0;i<=gjk->order;++i)		{ 
basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; 
}
			for( i=0;i<nfidx;++i,pfidx+=3)		{ 
basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); 
}
			for( i=0;i<neidx;++i,peidx+=4)		{ 
Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
			}
		if(0==nfaces)
			{
			sa->endBlock(sablock);
			return(depth);
			}
		/* Expand hull		*/
		for(;iterations<EPA_maxiterations;++iterations)
			{
			Face*		bf = FindBest();
			if(bf)
				{
				GJK::Mkv*	w = Support(-bf->n);
				const F		d(bf->n.dot(w->w)+bf->d);
				bestface	=	bf;
				if(d<-accuracy)
					{
					Face*	cf =0;
					Face*	ff =0;
					U		nf = 0;
					Detach(bf);
					bf->mark=++markid;
					for(U i=0;i<3;++i)	{ 
nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
					if(nf<=2)			{ break; }
					Link(cf,1,ff,2);
					} else break;
				} else break;
			}
		/* Extract contact	*/
		if(bestface)
			{
			const Vector3	b(GetCoordinates(bestface));
			normal			=	bestface->n;
			depth			=	Max<F>(0,bestface->d);
			for(U i=0;i<2;++i)
				{
				const F	s(F(i?-1:1));
				for(U j=0;j<3;++j)
					{
					features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i);
					}
				}
			nearest[0]		=	features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z();
			nearest[1]		=	features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z();
			} else failed=true;
		sa->endBlock(sablock);
		return(depth);
		}
	};
}

//
// Api
//

using namespace	gjkepa_impl;



//
bool	btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0,
								const btConvexShape *shape1,const btTransform &wtrs1,
								btScalar	radialmargin,
								btStackAlloc* stackAlloc,
								sResults&	results)
{
	

/* Initialize					*/
results.witnesses[0]	=
results.witnesses[1]	=
results.normal			=	Vector3(0,0,0);
results.depth			=	0;
results.status			=	sResults::Separated;
results.epa_iterations	=	0;
results.gjk_iterations	=	0;
/* Use GJK to locate origin		*/
GJK			gjk(stackAlloc,
				wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
				wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
				radialmargin+EPA_accuracy);
const Z		collide(gjk.SearchOrigin());
results.gjk_iterations	=	static_cast<int>(gjk.iterations+1);
if(collide)
	{
	/* Then EPA for penetration depth	*/
	EPA			epa(&gjk);
	const F		pd(epa.EvaluatePD());
	results.epa_iterations	=	static_cast<int>(epa.iterations+1);
	if(pd>0)
		{
		results.status			=	sResults::Penetrating;
		results.normal			=	epa.normal;
		results.depth			=	pd;
		results.witnesses[0]	=	epa.nearest[0];
		results.witnesses[1]	=	epa.nearest[1];
		return(true);
		} else { if(epa.failed) results.status=sResults::EPA_Failed; }
	} else { if(gjk.failed) results.status=sResults::GJK_Failed; }
return(false);
}