updated EPA,

updated user manual (work in progress)

Demos/OpenGL/DemoApplication.cpp

@@ -193,11 +193,15 @@ void DemoApplication::stepBack()
 }
 void DemoApplication::zoomIn() 
 { 
-	m_cameraDistance -= 1; updateCamera(); 
+	m_cameraDistance -= 0.4; updateCamera(); 
+	if (m_cameraDistance < 0.1)
+		m_cameraDistance = 0.1;
+
 }
 void DemoApplication::zoomOut() 
 { 
-	m_cameraDistance += 1; updateCamera(); 
+	m_cameraDistance += 0.4; updateCamera(); 
+	
 }
 
 

src/BulletCollision/CollisionShapes/btConeShape.cpp

@@ -16,50 +16,71 @@ subject to the following restrictions:
 #include "btConeShape.h"
 #include "LinearMath/btPoint3.h"
 
-#ifdef WIN32
+
-static int coneindices[3] = {1,2,0};
-#else
-static int coneindices[3] = {2,1,0};
-#endif
 
 btConeShape::btConeShape (btScalar radius,btScalar height):
 m_radius (radius),
 m_height(height)
 {
+	setConeUpIndex(1);
 	btVector3 halfExtents;
 	m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height));
 }
 
+///choose upAxis index
+void	btConeShape::setConeUpIndex(int upIndex)
+{
+	switch (upIndex)
+	{
+	case 0:
+			m_coneIndices[0] = 1;
+			m_coneIndices[1] = 0;
+			m_coneIndices[2] = 2;
+		break;
+	case 1:
+			m_coneIndices[0] = 0;
+			m_coneIndices[1] = 1;
+			m_coneIndices[2] = 2;
+		break;
+	case 2:
+			m_coneIndices[0] = 0;
+			m_coneIndices[1] = 2;
+			m_coneIndices[2] = 1;
+		break;
+	default:
+		assert(0);
+	};
+}
 
 btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
 {
 	
 	float halfHeight = m_height * 0.5f;
 
- if (v[coneindices[1]] > v.length() * m_sinAngle)
+ if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
  {
 	btVector3 tmp;
 
-	tmp[coneindices[0]] = 0.f;
+	tmp[m_coneIndices[0]] = 0.f;
-	tmp[coneindices[1]] = halfHeight;
+	tmp[m_coneIndices[1]] = halfHeight;
-	tmp[coneindices[2]] = 0.f;
+	tmp[m_coneIndices[2]] = 0.f;
 	return tmp;
  }
   else {
-    btScalar s = btSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]);
+    btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
     if (s > SIMD_EPSILON) {
       btScalar d = m_radius / s;
 	  btVector3 tmp;
-	  tmp[coneindices[0]] = v[coneindices[0]] * d;
+	  tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
-	  tmp[coneindices[1]] = -halfHeight;
+	  tmp[m_coneIndices[1]] = -halfHeight;
-	  tmp[coneindices[2]] = v[coneindices[2]] * d;
+	  tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
 	  return tmp;
     }
     else  {
 		btVector3 tmp;
-		tmp[coneindices[0]] = 0.f;
+		tmp[m_coneIndices[0]] = 0.f;
-		tmp[coneindices[1]] = -halfHeight;
+		tmp[m_coneIndices[1]] = -halfHeight;
-		tmp[coneindices[2]] = 0.f;
+		tmp[m_coneIndices[2]] = 0.f;
 		return tmp;
 	}
   }

src/BulletCollision/CollisionShapes/btConeShape.h

@@ -19,7 +19,7 @@ subject to the following restrictions:
 #include "btConvexShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
 
-/// implements cone shape interface
+///btConeShape implements a Cone shape, around the Y axis
 class btConeShape : public btConvexShape
 
 {
@@ -27,7 +27,7 @@ class btConeShape : public btConvexShape
 	float m_sinAngle;
 	float m_radius;
 	float m_height;
-	
+	int		m_coneIndices[3];
 	btVector3 coneLocalSupport(const btVector3& v) const;
 
 
@@ -76,8 +76,28 @@ public:
 		{
 			return "Cone";
 		}
+		
+		///choose upAxis index
+		void	setConeUpIndex(int upIndex);
+		
+		int	getConeUpIndex() const
+		{
+			return m_coneIndices[1];
+		}
 };
 
+///btConeShape implements a Cone shape, around the X axis
+class btConeShapeX : public btConeShape
+{
+	public:
+		btConeShapeX(btScalar radius,btScalar height);
+};
 
+///btConeShapeZ implements a Cone shape, around the Z axis
+class btConeShapeZ : public btConeShape
+{
+	public:
+		btConeShapeZ(btScalar radius,btScalar height);
+};
 #endif //CONE_MINKOWSKI_H
 

src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp

@@ -3,13 +3,19 @@ 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.
+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, 
+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.
+1. The origin of this software must not be misrepresented; you must not 
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+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.
 */
 
@@ -57,20 +63,24 @@ static const U			chkPrecision		=1/U(sizeof(F)==4);
 
 static const F			cstInf				=F(1/sin(0.));
 static const F			cstPi				=F(acos(-1.));
+static const F			cst2Pi				=cstPi*2;
 
 static const U			GJK_maxiterations	=128;
 static const U			GJK_hashsize		=1<<6;
 static const U			GJK_hashmask		=GJK_hashsize-1;
 static const F			GJK_insimplex_eps	=F(0.0001);
+static const F			GJK_sqinsimplex_eps	=GJK_insimplex_eps*GJK_insimplex_eps;
 
 static const U			EPA_maxiterations	=256;
-static const F			EPA_accuracy		=F(1./1000./* of meters*/);
+static const F			EPA_inface_eps		=F(0.01);
+static const F			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)	{ 
@@ -85,6 +95,18 @@ throw(object); }
 #else
 template <typename T> static inline void	Raise(const T&)				{}
 #endif
+static inline F								Det(const Vector3& a,const Vector3& b,const Vector3& 
+c,const Vector3& d)
+		{
+		return(	-(a.z()*b.y()*c.x()) + 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()*d.x() - a.y()*b.z()*d.x() - a.z()*c.y()*d.x() + 
+b.z()*c.y()*d.x() + a.y()*c.z()*d.x() - b.y()*c.z()*d.x() -
+				a.z()*b.x()*d.y() + a.x()*b.z()*d.y() + a.z()*c.x()*d.y() - 
+b.z()*c.x()*d.y() - a.x()*c.z()*d.y() + b.x()*c.z()*d.y() +
+				a.y()*b.x()*d.z() - a.x()*b.y()*d.z() - a.y()*c.x()*d.z() + 
+b.y()*c.x()*d.z() + a.x()*c.y()*d.z() - b.x()*c.y()*d.z());
+		}
 
 //
 // StackAlloc
@@ -196,6 +218,7 @@ struct	GJK
 	Mkv						simplex[5];
 	Vector3					ray;
 	U						order;
+	U						iterations;
 	F						margin;
 	Z						failed;
 	//
@@ -251,7 +274,7 @@ struct	GJK
 		if(ab.dot(ao)>=0)
 			{
 			const Vector3	cabo(cross(ab,ao));
-			if(cabo.length2()>GJK_insimplex_eps)
+			if(cabo.length2()>GJK_sqinsimplex_eps)
 				{ ray=cross(cabo,ab); }
 				else
 				{ return(true); }
@@ -270,9 +293,9 @@ ac)
 	inline Z		SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& 
 ac,const Vector3& cabc)
 		{
-				if((cross(cabc,ab)).dot(ao)<0)
+				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)>0)
+		else	if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps)
 			{ order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); }
 		else
 			{
@@ -307,13 +330,13 @@ order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab
 	//
 	inline Z		SearchOrigin(const Vector3& initray=Vector3(1,0,0))
 		{
-		static const U	maxiterations(128);
+		iterations	=	0;
-		U				iterations(maxiterations);
 		order		=	0;
 		failed		=	false;
 		Support(initray,simplex[0]);ray=-SPXW(0);
 		ClearMemory(table,sizeof(void*)*hashsize);
-		do	{
+		for(;iterations<GJK_maxiterations;++iterations)
+			{
 			const F		rl(ray.length());
 			ray/=rl>0?rl:1;
 			if(FetchSupport())
@@ -327,7 +350,7 @@ order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab
 					}
 				if(found) return(true);
 				} else return(false);
-			} while(--iterations);
+			}
 		failed=true;
 		return(false);
 		}
@@ -338,15 +361,31 @@ order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab
 			{
 			/* Point		*/
 			case	0:	break;
-			/* Line	TODO	*/
+			/* Line			*/
-			case	1:	break;
+			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[++order]);
+				Support( n,simplex[3]);
-				Support(-n,simplex[++order]);
+				Support(-n,simplex[4]);
+				order=4;
 				return(true);
 				}
 			break;
@@ -379,6 +418,17 @@ struct	EPA
 		Face*			next;
 		Face()			{}
 		};
+	//
+	GJK*			gjk;
+	StackAlloc*		sa;
+	Face*			root;
+	U				nfaces;
+	U				iterations;
+	Vector3			features[2][3];
+	Vector3			nearest[2];
+	Vector3			normal;
+	F				depth;
+	Z				failed;
 	//
 					EPA(GJK* pgjk)
 		{
@@ -415,29 +465,20 @@ struct	EPA
 		}
 	//
 	inline Z		Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* 
-c)
+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;
-		if(len>0)
+		f->n	=	nrm/(len>0?len:cstInf);
-			{
+		f->d	=	Max<F>(0,-f->n.dot(a->w));
-			f->n	=	nrm/len;
+		return(valid);
-			f->d	=	-f->n.dot(a->w);
-			return(	(cross(a->w,b->w).dot(nrm)>=0)&&
-					(cross(b->w,c->w).dot(nrm)>=0)&&
-					(cross(c->w,a->w).dot(nrm)>=0));
-			}
-			else
-			{
-			f->n	=	Vector3(1,0,0);
-			f->d	=	-cstInf;
-			invalid	=	true;
-			return(false);
-			}
 		}
 	//
 	inline Face*	NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
@@ -476,12 +517,19 @@ c)
 			}
 		}
 	//
-	inline void		Link(Face* f0,U e0,Face* f1,U e1)
+	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(sa->Allocate<GJK::Mkv>());
+		gjk->Support(w,*v);
+		return(v);
+		}
+	//
 	U				BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& 
 ff)
 		{
@@ -509,78 +557,67 @@ ff)
 		return(ne);
 		}
 	//
-	inline F		EvaluatePD(F accuracy=0.0001)
+	inline F		EvaluatePD(F accuracy=EPA_accuracy)
 		{
-		static const U		maxiterations(256);
 		StackAlloc::Block*	sablock(sa->BeginBlock());
-		U					iterations(0);
-		Face*				prevbestface(0);
 		Face*				bestface(0);
 		U					markid(1);
 		depth		=	-cstInf;
 		normal		=	Vector3(0,0,0);
 		root		=	0;
 		nfaces		=	0;
-		invalid	=	false;
+		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];
-			U				basecount(0);
 			switch(gjk->order)
 				{
-				case	3:
+				/* 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}};
-					for(U i=0;i<4;++i)	{ 
+							pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6;
-basemkv[i]=sa->Allocate<GJK::Mkv>();*basemkv[i]=gjk->simplex[i]; }
+							}	break;
-					for(U i=0;i<4;++i)	{ 
+				/* Hexahedron		*/
-basefaces[i]=NewFace(basemkv[fidx[i][0]],basemkv[fidx[i][1]],basemkv[fidx[i][2]]); 
+				case	4:	{
-}
-					for(U i=0;i<6;++i)	{ 
-Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); }
-					basecount=4;
-					}
-				break;
-				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}};
-					for(U i=0;i<5;++i)	{ 
+							pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9;
+							}	break;
+				}
+			for(U i=0;i<=gjk->order;++i)		{ 
 basemkv[i]=sa->Allocate<GJK::Mkv>();*basemkv[i]=gjk->simplex[i]; }
-					for(U i=0;i<6;++i)	{ 
+			for(U i=0;i<nfidx;++i,pfidx+=3)		{ 
-basefaces[i]=NewFace(basemkv[fidx[i][0]],basemkv[fidx[i][1]],basemkv[fidx[i][2]]); 
+basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); 
 }
-					for(U i=0;i<9;++i)	{ 
+			for(U i=0;i<neidx;++i,peidx+=4)		{ 
-Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); }
+Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
-					basecount=6;
 			}
-				break;
+		if(0==nfaces)
-				}
-			for(U i=0;i<basecount;++i)	{ if(basefaces[i]->d<0) { invalid=true;break; 
-} }
-			}
-		if(invalid||(0==nfaces))
 			{
 			sa->EndBlock(sablock);
 			return(depth);
 			}
 		/* Expand hull		*/
-		for(;iterations<maxiterations;++iterations)
+		for(;iterations<EPA_maxiterations;++iterations)
 			{
 			Face*		bf(FindBest());
 			if(bf)
 				{
-				GJK::Mkv*		w(sa->Allocate<GJK::Mkv>());
+				GJK::Mkv*	w(Support(-bf->n));
-				gjk->Support(-bf->n,*w);
+				const F		d(bf->n.dot(w->w)+bf->d);
-				prevbestface=bestface;
 				bestface	=	bf;
-				if((bf->n.dot(w->w)+bf->d)<-accuracy)
+				if(d<-accuracy)
 					{
 					Face*	cf(0);
 					Face*	ff(0);
@@ -589,7 +626,6 @@ Link(basefaces[eidx[i][0]],eidx[i][1],basefaces[eidx[i][2]],eidx[i][3]); }
 					bf->mark=++markid;
 					for(U i=0;i<3;++i)	{ 
 nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
-					if(invalid)	{ bestface=0;break; }
 					if(nf<=2)			{ break; }
 					Link(cf,1,ff,2);
 					} else break;
@@ -615,19 +651,7 @@ nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
 		sa->EndBlock(sablock);
 		return(depth);
 		}
-	//
-	GJK*			gjk;
-	StackAlloc*		sa;
-	Face*			root;
-	U				nfaces;
-	Vector3			features[2][3];
-	Vector3			nearest[2];
-	Vector3			normal;
-	F				depth;
-	Z				invalid;
-	Z				failed;
 	};
-
 }
 
 //
@@ -653,16 +677,21 @@ 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(&g_sa,
 				wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
 				wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
-				radialmargin);
+				radialmargin+EPA_accuracy);
-if(gjk.SearchOrigin())
+const Z		collide(gjk.SearchOrigin());
+results.gjk_iterations	=	gjk.iterations+1;
+if(collide)
 	{
 	/* Then EPA for penetration depth	*/
 	EPA			epa(&gjk);
 	const F		pd(epa.EvaluatePD());
+	results.epa_iterations	=	epa.iterations+1;
 	if(pd>0)
 		{
 		results.status			=	sResults::Penetrating;
@@ -676,3 +705,5 @@ if(gjk.SearchOrigin())
 return(false);
 }
 
+
+

src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h

@@ -38,6 +38,8 @@ struct	sResults
 	btVector3	witnesses[2];
 	btVector3	normal;
 	btScalar	depth;
+	int	epa_iterations;
+	int	gjk_iterations;
 	};
 static bool	Collide(btConvexShape* shape0,const btTransform& wtrs0,
 					btConvexShape* shape1,const btTransform& wtrs1,