Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @ 8474

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

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

Download in other formats: