Bug fix in EPA implementation, compute closest triangle to the origin using edges/vertices, instead of the projected origin distance.

See Issue 606 for detailed description. Thanks a lot to Jarno for the report and fix!

src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

@@ -511,7 +511,6 @@ namespace gjkepa2_impl
 		{
 			btVector3	n;
 			btScalar	d;
-			btScalar	p;
 			sSV*		c[3];
 			sFace*		f[3];
 			sFace*		l[2];
@@ -657,7 +656,7 @@ namespace gjkepa2_impl
 										remove(m_hull,best);
 										append(m_stock,best);
 										best=findbest();
-										if(best->p>=outer.p) outer=*best;
+										outer=*best;
 									} else { m_status=eStatus::InvalidHull;break; }
 								} else { m_status=eStatus::AccuraryReached;break; }
 							} else { m_status=eStatus::OutOfVertices;break; }
@@ -696,6 +695,42 @@ namespace gjkepa2_impl
 				m_result.p[0]=1;	
 				return(m_status);
 			}
+			bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
+			{
+				const btVector3 ba = b->w - a->w;
+				const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
+				const btScalar a_dot_nab = btDot(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 btScalar ba_l2 = ba.length2();
+					const btScalar a_dot_ba = btDot(a->w, ba);
+					const btScalar b_dot_ba = btDot(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 btScalar a_dot_b = btDot(a->w, b->w);
+						dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
+					}
+
+					return true;
+				}
+
+				return false;
+			}
 			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
 			{
 				if(m_stock.root)
@@ -710,41 +745,48 @@ namespace gjkepa2_impl
 					face->n		=	btCross(b->w-a->w,c->w-a->w);
 					const btScalar	l=face->n.length();
 					const bool		v=l>EPA_ACCURACY;
-					face->p		=	btMin(btMin(
+
-						btDot(a->w,btCross(face->n,a->w-b->w)),
-						btDot(b->w,btCross(face->n,b->w-c->w))),
-						btDot(c->w,btCross(face->n,c->w-a->w)))	/
-						(v?l:1);
-					face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
 					if(v)
 					{
-						face->d		=	btDot(a->w,face->n)/l;
+						if(!(getedgedist(face, a, b, face->d) ||
-						face->n		/=	l;
+							 getedgedist(face, b, c, face->d) ||
-						if(forced||(face->d>=-EPA_PLANE_EPS))
+							 getedgedist(face, c, a, face->d)))
 						{
-							return(face);
+							// Origin projects to the interior of the triangle
-						} else m_status=eStatus::NonConvex;
+							// Use distance to triangle plane
-					} else m_status=eStatus::Degenerated;
+							face->d = btDot(a->w, face->n) / l;
-					remove(m_hull,face);
+						}
-					append(m_stock,face);
+
-					return(0);
+						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;
+				m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
-				return(0);
+				return 0;
 			}
 			sFace*				findbest()
 			{
 				sFace*		minf=m_hull.root;
 				btScalar	mind=minf->d*minf->d;
-				btScalar	maxp=minf->p;
 				for(sFace* f=minf->l[1];f;f=f->l[1])
 				{
 					const btScalar	sqd=f->d*f->d;
-					if((f->p>=maxp)&&(sqd<mind))
+					if(sqd<mind)
 					{
 						minf=f;
 						mind=sqd;
-						maxp=f->p;
 					}
 				}
 				return(minf);