Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @ 6698

Last change on this file since 6698 was 5781, checked in by rgrieder, 15 years ago
Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
Property svn:eol-style set to `native`
File size: 23.6 KB

Rev	Line
[1963]	1	/*
	2	Bullet Continuous Collision Detection and Physics Library
	3	Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
	4
	5	This software is provided 'as-is', without any express or implied warranty.
	6	In no event will the authors be held liable for any damages arising from the
	7	use of this software.
	8	Permission is granted to anyone to use this software for any purpose,
	9	including commercial applications, and to alter it and redistribute it
	10	freely,
	11	subject to the following restrictions:
	12
	13	1. The origin of this software must not be misrepresented; you must not
	14	claim that you wrote the original software. If you use this software in a
	15	product, an acknowledgment in the product documentation would be appreciated
	16	but is not required.
	17	2. Altered source versions must be plainly marked as such, and must not be
	18	misrepresented as being the original software.
	19	3. This notice may not be removed or altered from any source distribution.
	20	*/
	21
	22	/*
	23	GJK-EPA collision solver by Nathanael Presson, 2008
	24	*/
	25	#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
	26	#include "BulletCollision/CollisionShapes/btSphereShape.h"
	27	#include "btGjkEpa2.h"
	28
	29	#if defined(DEBUG) \|\| defined (_DEBUG)
	30	#include <stdio.h> //for debug printf
	31	#ifdef __SPU__
	32	#include <spu_printf.h>
	33	#define printf spu_printf
	34	#endif //__SPU__
	35	#endif
	36
	37	namespace gjkepa2_impl
	38	{
	39
[2430]	40	// Config
[1963]	41
	42	/* GJK */
	43	#define GJK_MAX_ITERATIONS 128
	44	#define GJK_ACCURARY ((btScalar)0.0001)
	45	#define GJK_MIN_DISTANCE ((btScalar)0.0001)
	46	#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
	47	#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
	48	#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
	49	#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
	50
	51	/* EPA */
	52	#define EPA_MAX_VERTICES 64
	53	#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
	54	#define EPA_MAX_ITERATIONS 255
	55	#define EPA_ACCURACY ((btScalar)0.0001)
	56	#define EPA_FALLBACK (10*EPA_ACCURACY)
	57	#define EPA_PLANE_EPS ((btScalar)0.00001)
	58	#define EPA_INSIDE_EPS ((btScalar)0.01)
	59
	60
[2430]	61	// Shorthands
	62	typedef unsigned int U;
	63	typedef unsigned char U1;
[1963]	64
[2430]	65	// MinkowskiDiff
	66	struct MinkowskiDiff
[1963]	67	{
[2430]	68	const btConvexShape* m_shapes[2];
	69	btMatrix3x3 m_toshape1;
	70	btTransform m_toshape0;
	71	btVector3 (btConvexShape::*Ls)(const btVector3&) const;
	72	void EnableMargin(bool enable)
[1963]	73	{
[2430]	74	if(enable)
	75	Ls=&btConvexShape::localGetSupportVertexNonVirtual;
[1963]	76	else
[2430]	77	Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
[1963]	78	}
[2430]	79	inline btVector3 Support0(const btVector3& d) const
[1963]	80	{
[2430]	81	return(((m_shapes[0])->*(Ls))(d));
[1963]	82	}
[2430]	83	inline btVector3 Support1(const btVector3& d) const
[1963]	84	{
[2430]	85	return(m_toshape0((m_shapes[1])->(Ls))(m_toshape1*d));
[1963]	86	}
[2430]	87	inline btVector3 Support(const btVector3& d) const
[1963]	88	{
[2430]	89	return(Support0(d)-Support1(-d));
[1963]	90	}
[2430]	91	btVector3 Support(const btVector3& d,U index) const
[1963]	92	{
[2430]	93	if(index)
	94	return(Support1(d));
[1963]	95	else
[2430]	96	return(Support0(d));
[1963]	97	}
	98	};
	99
[2430]	100	typedef MinkowskiDiff tShape;
[1963]	101
	102
[2430]	103	// GJK
	104	struct GJK
[1963]	105	{
[2430]	106	/* Types */
	107	struct sSV
	108	{
	109	btVector3 d,w;
	110	};
	111	struct sSimplex
	112	{
	113	sSV* c[4];
	114	btScalar p[4];
	115	U rank;
	116	};
	117	struct eStatus { enum _ {
	118	Valid,
	119	Inside,
	120	Failed };};
	121	/* Fields */
	122	tShape m_shape;
	123	btVector3 m_ray;
	124	btScalar m_distance;
	125	sSimplex m_simplices[2];
	126	sSV m_store[4];
	127	sSV* m_free[4];
	128	U m_nfree;
	129	U m_current;
	130	sSimplex* m_simplex;
	131	eStatus::_ m_status;
	132	/* Methods */
	133	GJK()
	134	{
	135	Initialize();
[1963]	136	}
[2430]	137	void Initialize()
[1963]	138	{
[2430]	139	m_ray = btVector3(0,0,0);
	140	m_nfree = 0;
	141	m_status = eStatus::Failed;
	142	m_current = 0;
	143	m_distance = 0;
[1963]	144	}
[2430]	145	eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
[1963]	146	{
[2430]	147	U iterations=0;
	148	btScalar sqdist=0;
	149	btScalar alpha=0;
	150	btVector3 lastw[4];
	151	U clastw=0;
	152	/* Initialize solver */
	153	m_free[0] = &m_store[0];
	154	m_free[1] = &m_store[1];
	155	m_free[2] = &m_store[2];
	156	m_free[3] = &m_store[3];
	157	m_nfree = 4;
	158	m_current = 0;
	159	m_status = eStatus::Valid;
	160	m_shape = shapearg;
	161	m_distance = 0;
	162	/* Initialize simplex */
	163	m_simplices[0].rank = 0;
	164	m_ray = guess;
	165	const btScalar sqrl= m_ray.length2();
	166	appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
	167	m_simplices[0].p[0] = 1;
	168	m_ray = m_simplices[0].c[0]->w;
	169	sqdist = sqrl;
	170	lastw[0] =
	171	lastw[1] =
	172	lastw[2] =
	173	lastw[3] = m_ray;
	174	/* Loop */
	175	do {
	176	const U next=1-m_current;
	177	sSimplex& cs=m_simplices[m_current];
	178	sSimplex& ns=m_simplices[next];
	179	/* Check zero */
	180	const btScalar rl=m_ray.length();
	181	if(rl<GJK_MIN_DISTANCE)
	182	{/* Touching or inside */
	183	m_status=eStatus::Inside;
	184	break;
	185	}
	186	/* Append new vertice in -'v' direction */
	187	appendvertice(cs,-m_ray);
	188	const btVector3& w=cs.c[cs.rank-1]->w;
	189	bool found=false;
	190	for(U i=0;i<4;++i)
[1963]	191	{
[2430]	192	if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
	193	{ found=true;break; }
[1963]	194	}
[2430]	195	if(found)
	196	{/* Return old simplex */
	197	removevertice(m_simplices[m_current]);
	198	break;
	199	}
[1963]	200	else
[2430]	201	{/* Update lastw */
	202	lastw[clastw=(clastw+1)&3]=w;
	203	}
	204	/* Check for termination */
	205	const btScalar omega=dot(m_ray,w)/rl;
	206	alpha=btMax(omega,alpha);
	207	if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
	208	{/* Return old simplex */
	209	removevertice(m_simplices[m_current]);
	210	break;
	211	}
	212	/* Reduce simplex */
	213	btScalar weights[4];
	214	U mask=0;
	215	switch(cs.rank)
[1963]	216	{
[2430]	217	case 2: sqdist=projectorigin( cs.c[0]->w,
	218	cs.c[1]->w,
	219	weights,mask);break;
	220	case 3: sqdist=projectorigin( cs.c[0]->w,
	221	cs.c[1]->w,
	222	cs.c[2]->w,
	223	weights,mask);break;
	224	case 4: sqdist=projectorigin( cs.c[0]->w,
	225	cs.c[1]->w,
	226	cs.c[2]->w,
	227	cs.c[3]->w,
	228	weights,mask);break;
[1963]	229	}
[2430]	230	if(sqdist>=0)
	231	{/* Valid */
	232	ns.rank = 0;
	233	m_ray = btVector3(0,0,0);
	234	m_current = next;
	235	for(U i=0,ni=cs.rank;i<ni;++i)
	236	{
	237	if(mask&(1<<i))
	238	{
	239	ns.c[ns.rank] = cs.c[i];
	240	ns.p[ns.rank++] = weights[i];
	241	m_ray += cs.c[i]->w*weights[i];
	242	}
	243	else
	244	{
	245	m_free[m_nfree++] = cs.c[i];
	246	}
	247	}
	248	if(mask==15) m_status=eStatus::Inside;
	249	}
	250	else
	251	{/* Return old simplex */
	252	removevertice(m_simplices[m_current]);
	253	break;
	254	}
	255	m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
	256	} while(m_status==eStatus::Valid);
	257	m_simplex=&m_simplices[m_current];
	258	switch(m_status)
[1963]	259	{
[2430]	260	case eStatus::Valid: m_distance=m_ray.length();break;
	261	case eStatus::Inside: m_distance=0;break;
	262	}
	263	return(m_status);
[1963]	264	}
[2430]	265	bool EncloseOrigin()
[1963]	266	{
[2430]	267	switch(m_simplex->rank)
[1963]	268	{
[2430]	269	case 1:
[1963]	270	{
[2430]	271	for(U i=0;i<3;++i)
	272	{
	273	btVector3 axis=btVector3(0,0,0);
	274	axis[i]=1;
	275	appendvertice(*m_simplex, axis);
	276	if(EncloseOrigin()) return(true);
	277	removevertice(*m_simplex);
	278	appendvertice(*m_simplex,-axis);
	279	if(EncloseOrigin()) return(true);
	280	removevertice(*m_simplex);
	281	}
[1963]	282	}
[2430]	283	break;
	284	case 2:
	285	{
	286	const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
	287	for(U i=0;i<3;++i)
	288	{
	289	btVector3 axis=btVector3(0,0,0);
	290	axis[i]=1;
	291	const btVector3 p=cross(d,axis);
	292	if(p.length2()>0)
	293	{
	294	appendvertice(*m_simplex, p);
	295	if(EncloseOrigin()) return(true);
	296	removevertice(*m_simplex);
	297	appendvertice(*m_simplex,-p);
	298	if(EncloseOrigin()) return(true);
	299	removevertice(*m_simplex);
	300	}
	301	}
	302	}
	303	break;
	304	case 3:
	305	{
	306	const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
	307	m_simplex->c[2]->w-m_simplex->c[0]->w);
	308	if(n.length2()>0)
	309	{
	310	appendvertice(*m_simplex,n);
	311	if(EncloseOrigin()) return(true);
	312	removevertice(*m_simplex);
	313	appendvertice(*m_simplex,-n);
	314	if(EncloseOrigin()) return(true);
	315	removevertice(*m_simplex);
	316	}
	317	}
	318	break;
	319	case 4:
	320	{
	321	if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
	322	m_simplex->c[1]->w-m_simplex->c[3]->w,
	323	m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
	324	return(true);
	325	}
	326	break;
[1963]	327	}
[2430]	328	return(false);
[1963]	329	}
[2430]	330	/* Internals */
	331	void getsupport(const btVector3& d,sSV& sv) const
[1963]	332	{
[2430]	333	sv.d = d/d.length();
	334	sv.w = m_shape.Support(sv.d);
[1963]	335	}
[2430]	336	void removevertice(sSimplex& simplex)
[1963]	337	{
[2430]	338	m_free[m_nfree++]=simplex.c[--simplex.rank];
[1963]	339	}
[2430]	340	void appendvertice(sSimplex& simplex,const btVector3& v)
[1963]	341	{
[2430]	342	simplex.p[simplex.rank]=0;
	343	simplex.c[simplex.rank]=m_free[--m_nfree];
	344	getsupport(v,*simplex.c[simplex.rank++]);
	345	}
	346	static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
	347	{
	348	return( a.y()b.z()c.x()+a.z()b.x()c.y()-
	349	a.x()b.z()c.y()-a.y()b.x()c.z()+
	350	a.x()b.y()c.z()-a.z()b.y()c.x());
	351	}
	352	static btScalar projectorigin( const btVector3& a,
	353	const btVector3& b,
	354	btScalar* w,U& m)
	355	{
	356	const btVector3 d=b-a;
	357	const btScalar l=d.length2();
	358	if(l>GJK_SIMPLEX2_EPS)
[1963]	359	{
[2430]	360	const btScalar t(l>0?-dot(a,d)/l:0);
	361	if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
	362	else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
	363	else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
	364	}
	365	return(-1);
	366	}
	367	static btScalar projectorigin( const btVector3& a,
	368	const btVector3& b,
	369	const btVector3& c,
	370	btScalar* w,U& m)
	371	{
	372	static const U imd3[]={1,2,0};
	373	const btVector3* vt[]={&a,&b,&c};
	374	const btVector3 dl[]={a-b,b-c,c-a};
	375	const btVector3 n=cross(dl[0],dl[1]);
	376	const btScalar l=n.length2();
	377	if(l>GJK_SIMPLEX3_EPS)
	378	{
	379	btScalar mindist=-1;
	380	btScalar subw[2];
	381	U subm;
	382	for(U i=0;i<3;++i)
[1963]	383	{
[2430]	384	if(dot(*vt[i],cross(dl[i],n))>0)
	385	{
	386	const U j=imd3[i];
	387	const btScalar subd(projectorigin(vt[i],vt[j],subw,subm));
	388	if((mindist<0)\|\|(subd<mindist))
	389	{
	390	mindist = subd;
	391	m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
	392	w[i] = subw[0];
	393	w[j] = subw[1];
	394	w[imd3[j]] = 0;
	395	}
	396	}
[1963]	397	}
[2430]	398	if(mindist<0)
	399	{
	400	const btScalar d=dot(a,n);
	401	const btScalar s=btSqrt(l);
	402	const btVector3 p=n*(d/l);
	403	mindist = p.length2();
	404	m = 7;
	405	w[0] = (cross(dl[1],b-p)).length()/s;
	406	w[1] = (cross(dl[2],c-p)).length()/s;
	407	w[2] = 1-(w[0]+w[1]);
	408	}
	409	return(mindist);
[1963]	410	}
[2430]	411	return(-1);
[1963]	412	}
[2430]	413	static btScalar projectorigin( const btVector3& a,
	414	const btVector3& b,
	415	const btVector3& c,
	416	const btVector3& d,
	417	btScalar* w,U& m)
[1963]	418	{
[2430]	419	static const U imd3[]={1,2,0};
	420	const btVector3* vt[]={&a,&b,&c,&d};
	421	const btVector3 dl[]={a-d,b-d,c-d};
	422	const btScalar vl=det(dl[0],dl[1],dl[2]);
	423	const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0;
	424	if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
[1963]	425	{
[2430]	426	btScalar mindist=-1;
	427	btScalar subw[3];
	428	U subm;
	429	for(U i=0;i<3;++i)
[1963]	430	{
[2430]	431	const U j=imd3[i];
	432	const btScalar s=vl*dot(d,cross(dl[i],dl[j]));
	433	if(s>0)
	434	{
	435	const btScalar subd=projectorigin(vt[i],vt[j],d,subw,subm);
	436	if((mindist<0)\|\|(subd<mindist))
	437	{
	438	mindist = subd;
	439	m = static_cast<U>((subm&1?1<<i:0)+
[1963]	440	(subm&2?1<<j:0)+
	441	(subm&4?8:0));
[2430]	442	w[i] = subw[0];
	443	w[j] = subw[1];
	444	w[imd3[j]] = 0;
	445	w[3] = subw[2];
	446	}
	447	}
[1963]	448	}
[2430]	449	if(mindist<0)
	450	{
	451	mindist = 0;
	452	m = 15;
	453	w[0] = det(c,b,d)/vl;
	454	w[1] = det(a,c,d)/vl;
	455	w[2] = det(b,a,d)/vl;
	456	w[3] = 1-(w[0]+w[1]+w[2]);
	457	}
	458	return(mindist);
[1963]	459	}
[2430]	460	return(-1);
[1963]	461	}
	462	};
	463
[2430]	464	// EPA
	465	struct EPA
[1963]	466	{
[2430]	467	/* Types */
	468	typedef GJK::sSV sSV;
	469	struct sFace
[1963]	470	{
[2430]	471	btVector3 n;
	472	btScalar d;
	473	btScalar p;
	474	sSV* c[3];
	475	sFace* f[3];
	476	sFace* l[2];
	477	U1 e[3];
	478	U1 pass;
	479	};
	480	struct sList
[1963]	481	{
[2430]	482	sFace* root;
	483	U count;
	484	sList() : root(0),count(0) {}
	485	};
	486	struct sHorizon
	487	{
	488	sFace* cf;
	489	sFace* ff;
	490	U nf;
	491	sHorizon() : cf(0),ff(0),nf(0) {}
	492	};
	493	struct eStatus { enum _ {
	494	Valid,
	495	Touching,
	496	Degenerated,
	497	NonConvex,
	498	InvalidHull,
	499	OutOfFaces,
	500	OutOfVertices,
	501	AccuraryReached,
	502	FallBack,
	503	Failed };};
	504	/* Fields */
	505	eStatus::_ m_status;
	506	GJK::sSimplex m_result;
	507	btVector3 m_normal;
	508	btScalar m_depth;
	509	sSV m_sv_store[EPA_MAX_VERTICES];
	510	sFace m_fc_store[EPA_MAX_FACES];
	511	U m_nextsv;
	512	sList m_hull;
	513	sList m_stock;
	514	/* Methods */
	515	EPA()
	516	{
	517	Initialize();
	518	}
[1963]	519
[2430]	520
	521	static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
[1963]	522	{
[2430]	523	fa->e[ea]=(U1)eb;fa->f[ea]=fb;
	524	fb->e[eb]=(U1)ea;fb->f[eb]=fa;
[1963]	525	}
[2430]	526	static inline void append(sList& list,sFace* face)
[1963]	527	{
[2430]	528	face->l[0] = 0;
	529	face->l[1] = list.root;
	530	if(list.root) list.root->l[0]=face;
	531	list.root = face;
	532	++list.count;
[1963]	533	}
[2430]	534	static inline void remove(sList& list,sFace* face)
	535	{
	536	if(face->l[1]) face->l[1]->l[0]=face->l[0];
	537	if(face->l[0]) face->l[0]->l[1]=face->l[1];
	538	if(face==list.root) list.root=face->l[1];
	539	--list.count;
	540	}
	541
	542
	543	void Initialize()
	544	{
	545	m_status = eStatus::Failed;
	546	m_normal = btVector3(0,0,0);
	547	m_depth = 0;
	548	m_nextsv = 0;
	549	for(U i=0;i<EPA_MAX_FACES;++i)
	550	{
	551	append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
	552	}
	553	}
	554	eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
	555	{
	556	GJK::sSimplex& simplex=*gjk.m_simplex;
	557	if((simplex.rank>1)&&gjk.EncloseOrigin())
	558	{
	559
	560	/* Clean up */
	561	while(m_hull.root)
	562	{
	563	sFace* f = m_hull.root;
	564	remove(m_hull,f);
	565	append(m_stock,f);
	566	}
	567	m_status = eStatus::Valid;
	568	m_nextsv = 0;
	569	/* Orient simplex */
	570	if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
	571	simplex.c[1]->w-simplex.c[3]->w,
	572	simplex.c[2]->w-simplex.c[3]->w)<0)
	573	{
	574	btSwap(simplex.c[0],simplex.c[1]);
	575	btSwap(simplex.p[0],simplex.p[1]);
	576	}
	577	/* Build initial hull */
	578	sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
[1963]	579	newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
	580	newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
	581	newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
[2430]	582	if(m_hull.count==4)
	583	{
	584	sFace* best=findbest();
	585	sFace outer=*best;
	586	U pass=0;
	587	U iterations=0;
	588	bind(tetra[0],0,tetra[1],0);
	589	bind(tetra[0],1,tetra[2],0);
	590	bind(tetra[0],2,tetra[3],0);
	591	bind(tetra[1],1,tetra[3],2);
	592	bind(tetra[1],2,tetra[2],1);
	593	bind(tetra[2],2,tetra[3],1);
	594	m_status=eStatus::Valid;
	595	for(;iterations<EPA_MAX_ITERATIONS;++iterations)
[1963]	596	{
[2430]	597	if(m_nextsv<EPA_MAX_VERTICES)
	598	{
	599	sHorizon horizon;
	600	sSV* w=&m_sv_store[m_nextsv++];
	601	bool valid=true;
	602	best->pass = (U1)(++pass);
	603	gjk.getsupport(best->n,*w);
	604	const btScalar wdist=dot(best->n,w->w)-best->d;
	605	if(wdist>EPA_ACCURACY)
	606	{
	607	for(U j=0;(j<3)&&valid;++j)
	608	{
	609	valid&=expand( pass,w,
[1963]	610	best->f[j],best->e[j],
	611	horizon);
[2430]	612	}
	613	if(valid&&(horizon.nf>=3))
	614	{
	615	bind(horizon.cf,1,horizon.ff,2);
	616	remove(m_hull,best);
	617	append(m_stock,best);
	618	best=findbest();
	619	if(best->p>=outer.p) outer=*best;
	620	} else { m_status=eStatus::InvalidHull;break; }
	621	} else { m_status=eStatus::AccuraryReached;break; }
	622	} else { m_status=eStatus::OutOfVertices;break; }
	623	}
	624	const btVector3 projection=outer.n*outer.d;
	625	m_normal = outer.n;
	626	m_depth = outer.d;
	627	m_result.rank = 3;
	628	m_result.c[0] = outer.c[0];
	629	m_result.c[1] = outer.c[1];
	630	m_result.c[2] = outer.c[2];
	631	m_result.p[0] = cross( outer.c[1]->w-projection,
	632	outer.c[2]->w-projection).length();
	633	m_result.p[1] = cross( outer.c[2]->w-projection,
	634	outer.c[0]->w-projection).length();
	635	m_result.p[2] = cross( outer.c[0]->w-projection,
	636	outer.c[1]->w-projection).length();
	637	const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
	638	m_result.p[0] /= sum;
	639	m_result.p[1] /= sum;
	640	m_result.p[2] /= sum;
	641	return(m_status);
	642	}
[1963]	643	}
[2430]	644	/* Fallback */
	645	m_status = eStatus::FallBack;
	646	m_normal = -guess;
	647	const btScalar nl=m_normal.length();
	648	if(nl>0)
	649	m_normal = m_normal/nl;
	650	else
	651	m_normal = btVector3(1,0,0);
	652	m_depth = 0;
	653	m_result.rank=1;
	654	m_result.c[0]=simplex.c[0];
	655	m_result.p[0]=1;
	656	return(m_status);
[1963]	657	}
[2430]	658	sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
[1963]	659	{
[2430]	660	if(m_stock.root)
[1963]	661	{
[2430]	662	sFace* face=m_stock.root;
	663	remove(m_stock,face);
	664	append(m_hull,face);
	665	face->pass = 0;
	666	face->c[0] = a;
	667	face->c[1] = b;
	668	face->c[2] = c;
	669	face->n = cross(b->w-a->w,c->w-a->w);
	670	const btScalar l=face->n.length();
	671	const bool v=l>EPA_ACCURACY;
	672	face->p = btMin(btMin(
	673	dot(a->w,cross(face->n,a->w-b->w)),
	674	dot(b->w,cross(face->n,b->w-c->w))),
	675	dot(c->w,cross(face->n,c->w-a->w))) /
	676	(v?l:1);
	677	face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
	678	if(v)
	679	{
	680	face->d = dot(a->w,face->n)/l;
	681	face->n /= l;
	682	if(forced\|\|(face->d>=-EPA_PLANE_EPS))
	683	{
	684	return(face);
	685	} else m_status=eStatus::NonConvex;
	686	} else m_status=eStatus::Degenerated;
	687	remove(m_hull,face);
	688	append(m_stock,face);
	689	return(0);
	690	}
	691	m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
	692	return(0);
[1963]	693	}
[2430]	694	sFace* findbest()
[1963]	695	{
[2430]	696	sFace* minf=m_hull.root;
	697	btScalar mind=minf->d*minf->d;
	698	btScalar maxp=minf->p;
	699	for(sFace* f=minf->l[1];f;f=f->l[1])
[1963]	700	{
[2430]	701	const btScalar sqd=f->d*f->d;
	702	if((f->p>=maxp)&&(sqd<mind))
	703	{
	704	minf=f;
	705	mind=sqd;
	706	maxp=f->p;
	707	}
[1963]	708	}
[2430]	709	return(minf);
[1963]	710	}
[2430]	711	bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
[1963]	712	{
[2430]	713	static const U i1m3[]={1,2,0};
	714	static const U i2m3[]={2,0,1};
	715	if(f->pass!=pass)
[1963]	716	{
[2430]	717	const U e1=i1m3[e];
	718	if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
	719	{
	720	sFace* nf=newface(f->c[e1],f->c[e],w,false);
	721	if(nf)
	722	{
	723	bind(nf,0,f,e);
	724	if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
	725	horizon.cf=nf;
	726	++horizon.nf;
	727	return(true);
	728	}
	729	}
	730	else
	731	{
	732	const U e2=i2m3[e];
	733	f->pass = (U1)pass;
	734	if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
	735	expand(pass,w,f->f[e2],f->e[e2],horizon))
	736	{
	737	remove(m_hull,f);
	738	append(m_stock,f);
	739	return(true);
	740	}
	741	}
[1963]	742	}
[2430]	743	return(false);
[1963]	744	}
	745
[2430]	746	};
[1963]	747
[2430]	748	//
	749	static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
	750	const btConvexShape* shape1,const btTransform& wtrs1,
	751	btGjkEpaSolver2::sResults& results,
	752	tShape& shape,
	753	bool withmargins)
	754	{
	755	/* Results */
	756	results.witnesses[0] =
	757	results.witnesses[1] = btVector3(0,0,0);
	758	results.status = btGjkEpaSolver2::sResults::Separated;
	759	/* Shape */
	760	shape.m_shapes[0] = shape0;
	761	shape.m_shapes[1] = shape1;
	762	shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
	763	shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
	764	shape.EnableMargin(withmargins);
	765	}
[1963]	766
	767	}
	768
	769	//
	770	// Api
	771	//
	772
	773	using namespace gjkepa2_impl;
	774
	775	//
	776	int btGjkEpaSolver2::StackSizeRequirement()
	777	{
[2430]	778	return(sizeof(GJK)+sizeof(EPA));
[1963]	779	}
	780
	781	//
	782	bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
[2430]	783	const btTransform& wtrs0,
	784	const btConvexShape* shape1,
	785	const btTransform& wtrs1,
	786	const btVector3& guess,
	787	sResults& results)
[1963]	788	{
[2430]	789	tShape shape;
	790	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
	791	GJK gjk;
	792	GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
	793	if(gjk_status==GJK::eStatus::Valid)
[1963]	794	{
[2430]	795	btVector3 w0=btVector3(0,0,0);
	796	btVector3 w1=btVector3(0,0,0);
	797	for(U i=0;i<gjk.m_simplex->rank;++i)
[1963]	798	{
[2430]	799	const btScalar p=gjk.m_simplex->p[i];
	800	w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
	801	w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
[1963]	802	}
[2430]	803	results.witnesses[0] = wtrs0*w0;
	804	results.witnesses[1] = wtrs0*w1;
	805	results.normal = w0-w1;
	806	results.distance = results.normal.length();
	807	results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
	808	return(true);
[1963]	809	}
	810	else
	811	{
[2430]	812	results.status = gjk_status==GJK::eStatus::Inside?
	813	sResults::Penetrating :
	814	sResults::GJK_Failed ;
	815	return(false);
[1963]	816	}
	817	}
	818
	819	//
	820	bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
[2430]	821	const btTransform& wtrs0,
	822	const btConvexShape* shape1,
	823	const btTransform& wtrs1,
	824	const btVector3& guess,
	825	sResults& results,
	826	bool usemargins)
[1963]	827	{
[2430]	828	tShape shape;
	829	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
	830	GJK gjk;
	831	GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
	832	switch(gjk_status)
[1963]	833	{
	834	case GJK::eStatus::Inside:
	835	{
[2430]	836	EPA epa;
	837	EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
	838	if(epa_status!=EPA::eStatus::Failed)
[1963]	839	{
[2430]	840	btVector3 w0=btVector3(0,0,0);
	841	for(U i=0;i<epa.m_result.rank;++i)
[1963]	842	{
[2430]	843	w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
[1963]	844	}
[2430]	845	results.status = sResults::Penetrating;
	846	results.witnesses[0] = wtrs0*w0;
	847	results.witnesses[1] = wtrs0(w0-epa.m_normalepa.m_depth);
	848	results.normal = -epa.m_normal;
	849	results.distance = -epa.m_depth;
	850	return(true);
[1963]	851	} else results.status=sResults::EPA_Failed;
	852	}
[2430]	853	break;
[1963]	854	case GJK::eStatus::Failed:
[2430]	855	results.status=sResults::GJK_Failed;
	856	break;
[1963]	857	}
[2430]	858	return(false);
[1963]	859	}
	860
	861	//
	862	btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
	863	btScalar margin,
	864	const btConvexShape* shape0,
	865	const btTransform& wtrs0,
	866	sResults& results)
	867	{
[2430]	868	tShape shape;
	869	btSphereShape shape1(margin);
	870	btTransform wtrs1(btQuaternion(0,0,0,1),position);
	871	Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
	872	GJK gjk;
	873	GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
	874	if(gjk_status==GJK::eStatus::Valid)
[1963]	875	{
[2430]	876	btVector3 w0=btVector3(0,0,0);
	877	btVector3 w1=btVector3(0,0,0);
	878	for(U i=0;i<gjk.m_simplex->rank;++i)
[1963]	879	{
[2430]	880	const btScalar p=gjk.m_simplex->p[i];
	881	w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
	882	w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
[1963]	883	}
[2430]	884	results.witnesses[0] = wtrs0*w0;
	885	results.witnesses[1] = wtrs0*w1;
	886	const btVector3 delta= results.witnesses[1]-
	887	results.witnesses[0];
	888	const btScalar margin= shape0->getMarginNonVirtual()+
	889	shape1.getMarginNonVirtual();
	890	const btScalar length= delta.length();
	891	results.normal = delta/length;
	892	results.witnesses[0] += results.normal*margin;
	893	return(length-margin);
[1963]	894	}
	895	else
	896	{
[2430]	897	if(gjk_status==GJK::eStatus::Inside)
[1963]	898	{
[2430]	899	if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
[1963]	900	{
[2430]	901	const btVector3 delta= results.witnesses[0]-
	902	results.witnesses[1];
	903	const btScalar length= delta.length();
	904	if (length >= SIMD_EPSILON)
	905	results.normal = delta/length;
	906	return(-length);
[1963]	907	}
	908	}
	909	}
[2430]	910	return(SIMD_INFINITY);
[1963]	911	}
	912
	913	//
	914	bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
	915	const btTransform& wtrs0,
	916	const btConvexShape* shape1,
	917	const btTransform& wtrs1,
	918	const btVector3& guess,
	919	sResults& results)
	920	{
[2430]	921	if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
	922	return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
[1963]	923	else
[2430]	924	return(true);
[1963]	925	}
	926
	927	/* Symbols cleanup */
	928
	929	#undef GJK_MAX_ITERATIONS
	930	#undef GJK_ACCURARY
	931	#undef GJK_MIN_DISTANCE
	932	#undef GJK_DUPLICATED_EPS
	933	#undef GJK_SIMPLEX2_EPS
	934	#undef GJK_SIMPLEX3_EPS
	935	#undef GJK_SIMPLEX4_EPS
	936
	937	#undef EPA_MAX_VERTICES
	938	#undef EPA_MAX_FACES
	939	#undef EPA_MAX_ITERATIONS
	940	#undef EPA_ACCURACY
	941	#undef EPA_FALLBACK
	942	#undef EPA_PLANE_EPS
	943	#undef EPA_INSIDE_EPS

Note: See TracBrowser for help on using the repository browser.

Download in other formats: