EPA Simplification - Side-stepping Robustness Issues

[JessT]

G'Day all,

I'm at that stage in my physics engine where I need to get the Penetration Depth of two colliding polyhedra.
So, I've written a GJK implementation based on Casey's geometric simplification (along with suggestions from Rory, etc, in this thread)

Now, I am using Jay's method of finding the penetration depth along the relative motion vector (as described here and here), but this fails when there is no relative velocity between the bodies.
So, I am falling back on EPA.

I've been looking at the algorithm Gino gave in his book (Collision Detection in Interactive 3D Environments), and it's all pretty straight forward. Except for the robustness issues which he notes (pg 162) relating mostly to numerical stability in the calculations.

Well, after thinking about it, I think I can see a way (based on the geometric simplifications of GJK by Casey) that none of those numerical / robustness issues become apparent.

Firstly, I'll give a run-down of the algorithm so we're all on the same page;

1. Start with a tetrahedron (I'll call this the Shape) containing the Origin of the Minkowski Sum A + (-B) such as the one from GJK's termination (I'll skip over the part of generating a tetrahedron from the point, line-segment or triangle cases and just assume you have already done this).
2. Find the plane defined by a triangle on the Shape which is closest to the origin.
3. If the projected point of the origin does not lie within the triangle on the plane, reject and go to 2.
4. Using the line from the Origin through the projected point on the plane as the search direction, call w = Support(A + (-B)).
5. If the newly found point is less than some tolerance, Epsilon, further along the line, then go to 9.
6. Split the triangle by adding w.
7. Find the convex hull of the Shape using the Flood Fill algorithm for forward facing adjacent triangles (as looking from w toward the origin).
8. Go to 2.
9. We have reached the boundary of the Minkowski Sum, so this is the final feature - ie, the shortest penetration depth.

As Gino points out, the main concerns are within steps 2 & 3. These are where the barycentric coordinates are calculated, and hence the closest point.

What I propose is that you do not need to perform any of those calculations (much like Casey's simplification of GJK).

Step 2 can be calculated by a simple dot product with the triangle's normal.
Let's assume we have a left-hand wound system where all triangles are facing away from the origin and each triangle is defined by points A, B, C. The normal to the triangle is then n = Cross(B-A, C-A). The distance of the origin to the plane is trivially d = Dot(n, A).
(Note here that strictly speaking it is Dot(n, -A), however, this would give us a negative distance as our normals are pointing away from the origin. So, to compensate, we simply negate the value.)
When we calculate this distance, we should then cache the normal (n) for each triangle as it will be used again later.

Step 3 then is the point where all the Numerical stability issues come in to play. Here, we need to calculate the barycentric coordinates (ie, parameterized lines) for the projection of the origin onto the plane defined by the triangle w.r.t. a point on the triangle.

This is where the simplification comes in.
Following almost exactly the same logic Casey gave for his GJK implementation, we can see that for a perpendicular point to lie within the triangle, it must be within all three edges.
To test this, we simply take the dot product of the direction of the origin from a point on the edge with the perpendicular line from that edge.
Eg, for edge AB, we get the perpendicular line in the plane of the triangle by p = Cross(AB, n), then find a = Dot(p, -A).
If a > 0, the point lies on the wrong sided of the edge. Otherwise, repeat for the other two edges.

Diagram 1.
(Note that we can simplify the calculation by taking a = Dot(p, A) and checking for a <= 0 being on the wrong side of the edge).

This avoids having to calculate the barycentric coordinates or the exact point which is closest to the origin.

So, how do we do Step 4?
Well, Gino makes the requirement that the projected point on the plane must lie within the triangle. He then uses the line through the projected point from the origin as the search direction.
Since we know that this point is the closest point on the plane which is simply the orthogonal distance, this line must be orthogonal to the plane, which is simply the plane's normal (told you we'd need it again ).

Finally, this then invalidates the terminating condition (Step 5), as we no longer have the point on the plane nor the barycentric coordinates.
Again, taking straight from Casey's GJK simplification, we can trivially see if the new support point is further along in the search direction than the current triangle. As a robustness check, we then cap the maximum iterations we can make to catch oscillating between two collinear triangles as suggested by melax here.

As you can see, this by-passes all the numerical issues in Gino's original implementation.

I hope this will come in useful for anyone wanting to implement a fast, efficient Expanding Polytope Algorithm.

- Jess.

PS. Apologies for my original brief post - I had to rewrite this after losing the post the first time around.

[h4tt3n]

Nice. I also like that this is an "early out" method, since you know for sure the point is outside the triangle if just one of the face normal dot products are positive. *snatches idea and implements it into own code*

Cheers,
Michael

[JessT]

Ok, I've re-written my first post with all the proofs and working out.

Any comments, mistakes, discussion?

[Nathanael]

Have you compared your ideas with Bullet EPA implementation ? (btGjkEpa2.cpp)

Nathanael.

[JessT]

I hadn't, no...

After going over the code (Really hard to follow, btw, next to no comments and some obscure variable names!), it looks like this is where the magic happens;

Code: Select all

for(;iterations<EPA_MAX_ITERATIONS;++iterations)
{
	if(m_nextsv<EPA_MAX_VERTICES)
	{	
		sHorizon		horizon;
		sSV*			w=&m_sv_store[m_nextsv++];
		bool			valid=true;					
		best->pass	=	(U1)(++pass);
		gjk.getsupport(best->n,*w);
		const btScalar	wdist=dot(best->n,w->w)-best->d;
		if(wdist>EPA_ACCURACY)
		{
			for(U j=0;(j<3)&&valid;++j)
			{
				valid&=expand(	pass,w,
					best->f[j],best->e[j],
					horizon);
			}
			if(valid&&(horizon.nf>=3))
			{
				bind(horizon.cf,1,horizon.ff,2);
				remove(m_hull,best);
				append(m_stock,best);
				best=findbest();
				if(best->p>=outer.p) outer=*best;
			} else { m_status=eStatus::InvalidHull;break; }
		} else { m_status=eStatus::AccuraryReached;break; }
	} else { m_status=eStatus::OutOfVertices;break; }
}

So, it looks like you're doing much the same as I proposed. However, you still seem to be relying on a distance calculation of some sort to determine if the new support point should become part of the hull (I'm not sure about Bullet's stance on analytical convex shapes, but I would thank they would be handled differently?). Namely this line;

Code: Select all

const btScalar	wdist=dot(best->n,w->w)-best->d;

I am having a hard time understanding what the sSV struct's w member (w->w) and what sFace struct's d member (best->d) is.

Just by looking at it, it seems you're storing the square distance of 'best' from the origin in ->d, and the actual direction of w in ->w. You then find the total squared distance from the origin to w, and subtract the squared distance of best, leaving you with the squared distance of w from the plane best.

Why not simply perform the dot product on best->n and w-> and test for >= EPA_ACCURACY?

Also, on a side note, after looking through your GJK implementation, it would appear that you are performing quite a number of divides per loop.
Infact, it looks like you're still using Gino's implementation for checking distances instead of using the geometrical interpretation. Unless of course I'm missing something!

I'll see what I can do about hot-plugging the Bullet version into my own to compare. However, I am running my examples on embedded hardware with no FPU, so the results may be a little off.