My EPA doesn't work for spheres + capsules, why?

carlschissler · **Posted:** Tue Jul 03, 2012 9:01 pm

I'm developing a physics engine and I've already got implementations of the GJK and EPA algorithms that seem to work well for convex polyhedra and cylinders but which strangely produce erroneous results for capsule and sphere shapes, causing the simulation to blow up. As far as I know, these algorithms should work fine for any convex shapes, given a proper support function.

In testing, I tried generating a convex hull from a few thousand points on the surface of a sphere and it worked correctly. Since it seems like the EPA works better with those imperfect support points, I also tried randomly biasing the search direction in the sphere support function by a small amount and that improved the results but they were not 100% ok. It seems like my EPA is generating degenerate triangles for these curved surfaces but I can't figure out why.

I should mention that after running EPA, I use the triangle closest to the origin in the expanding hull to generate the final contact points. I project the origin onto this triangle and find the barycentric coordinates of that point, then use these coordinates to find the collision point on each object in world space. I'm getting degenerate cases where two of the vertices in the final EPA triangle are coincident (but not logically the same), thus producing lots of NaN's and exploding simulations.

Has anyone else encountered problems like these and have any idea what is going on?

Erwin Coumans · **Posted:** Wed Jul 04, 2012 5:13 pm

I've encountered numerous issues with GJK and EPA, so it is tricky to tell without further information what the problem is.
In my case I end up using a collision margin, and use GJK without the margin and EPA with the margin (and compensate afterwards).

Can you provide more information about the configuration in the degenerate cases? Two spheres of what radius and what positions?
What kind of termination criteria are you using in GJK and EPA?

Thanks,
Erwin

By the way, although it is not a solution but a workaround: for spheres and capsules you can use points and line segments instead for GJK/EPA and set the entire radius as margin.
You won't be able to use non-uniform scaling in that case though.

carlschissler · **Posted:** Fri Jul 06, 2012 12:49 am

Ok I think I've figured out that the problem is.

It turns out that because of the continuous nature of the sphere support mapping function, it was easy to generate simplex vertices that formed a triangle that was very close to the minkowski origin. Because of this, the normals of the triangle planes were being calculated incorrectly (because I made sure they always pointed away from the origin), such that they sometimes pointed towards the center of the polytope due to poor precision, producing weird results once the expansion proceeded.

In addition, because the normals faced inward sometimes, it would result in degenerate triangles being formed with 2 identical vertices - this is what was causing my simulation to blow up.

My solution: Every time a triangle is added to the expanding polytope, find a vertex that is not part of the triangle (should be able to find one in constant time from the first 4 vertices), and make sure that the normal points away from it. This should keep the normals facing outward in all cases because the polytope is convex.

mikeshafer · **Joined:** Fri Feb 24, 2012 10:45 am **Posts:** 41

Not sure this will help or not, but this is the function I came up with on my own and doing some research. It works pretty well for cubes, I have not tested it for other convex objects:

Code:

#pragma warning(disable: 4700)
void MEHGJKCollision::FindClosestPointInSimplex(MEHArray<MEHGJKPoint, 4> &simplexPoints, MEHVector3 &outContactPoint)
{
   switch (simplexPoints.GetSize())
   {
      case 1:
      {
         // the point on the simplex closest to the origin, is the point itself
         outContactPoint = simplexPoints[0].minowskiPoint;
         simplexPoints[0].barycentricCoord = 1.0;
      }
      break;
      
      case 2:
      {
         // we have a line
         MEHVector3 &start = simplexPoints[0].minowskiPoint;
         MEHVector3 &end = simplexPoints[1].minowskiPoint;
         
         MEHVector3 endToOrigin = end;
         endToOrigin *= (scalar)-1.0;
                     
         MEHVector3 endToStart = start;
         endToStart -= end;
         
         // the parametric value for the intersection is (p-q).u / u.u
         // we can check for zero/negative without doing the denominator, so let's do that first
         scalar numerator = endToOrigin.DotProductWith(endToStart);
         if (numerator <= 0.0)
         {
            // it's closer to end point, so remove the start point
            outContactPoint = simplexPoints[1].minowskiPoint;
            simplexPoints.Remove(0);
            simplexPoints[0].barycentricCoord = 1.0;
            break;
         }
         
         // check for the other side
         scalar denominator = endToStart.DotProductWith(endToStart);
         if (denominator <= numerator)
         {
            // it's closer to start point, so remove the end point
            outContactPoint = simplexPoints[0].minowskiPoint;
            simplexPoints.Remove(1);
            simplexPoints[0].barycentricCoord = 1.0;
            break;
         }
         
         // the point on the simplex closest to the origin, is on the line segment
         scalar t = numerator / denominator;
         outContactPoint = endToStart;
         outContactPoint *= t;
         outContactPoint += end;
         simplexPoints[0].barycentricCoord = t;
         simplexPoints[1].barycentricCoord = (scalar)1.0 - t;
      }
      break;
      
      case 3:
      {
         MEHVector3 &a = simplexPoints[0].minowskiPoint;
         MEHVector3 &b = simplexPoints[1].minowskiPoint;
         MEHVector3 &c = simplexPoints[2].minowskiPoint;
         MEHVector3 &originToa = a;
         MEHVector3 &originTob = b;
         MEHVector3 &originToc = c;
         
         scalar originToaDotaTob;
         scalar aTobDotOriginTob;
         scalar cTobDotOriginToc;
                              
         MEHVector3 aTob = b;
         aTob -= a;
         MEHVector3 cTob = b;
         cTob -= c;
         MEHVector3 cToa = a;
         cToa -= c;
         
         scalar aTobDotaTob = aTob.Magnitude2();
         scalar cTobDotcTob = cTob.Magnitude2();
         scalar cToaDotcToa = cToa.Magnitude2();
         
         // take care of degeneracy
         if (aTobDotaTob < MEHEPSILONSMALL)
         {
            MEHArray<MEHGJKPoint, 4> simplex;
            *simplex.GetFreePlace() = simplexPoints[0];
            *simplex.GetFreePlace() = simplexPoints[2];            
            FindClosestPointInSimplex(simplex, outContactPoint);
            simplexPoints = simplex;
            break;
         }
         if (cTobDotcTob < MEHEPSILONSMALL || cToaDotcToa < MEHEPSILONSMALL)
         {
            MEHArray<MEHGJKPoint, 4> simplex;
            *simplex.GetFreePlace() = simplexPoints[0];
            *simplex.GetFreePlace() = simplexPoints[1];            
            FindClosestPointInSimplex(simplex, outContactPoint);
            simplexPoints = simplex;
            break;
         }  
         
         // vert A
         
         originToaDotaTob = originToa.DotProductWith(aTob);
                  
         if (originToaDotaTob >= 0.0)
         {                        
            if (originToa.DotProductWith(cToa) <= 0.0)
            {
               // a is closest point, remove the rest
               outContactPoint = simplexPoints[0].minowskiPoint;
               simplexPoints.Remove(2);
               simplexPoints.Remove(1);
               simplexPoints[0].barycentricCoord = 1.0;
               break;
            }             
         }
                  
         MEHVector3 normal = cTob;         
         normal.ToCrossProductWith(aTob);                  
         
         MEHVector3 originTobDir = originTob;
         originTobDir.Normalize();
         
         // edge BA
         // if we got here, we can assume that originToa.DotProductWith(aTob) < 0.0                          
         MEHVector3 negbaNorm = normal;
         negbaNorm.ToCrossProductWith(aTob);
         scalar normMag = negbaNorm.Magnitude();
         scalar normDP;
         
         aTobDotaTob = aTob.Magnitude2();
         aTobDotOriginTob = aTob.DotProductWith(originTob);
         
         if (normMag > MEHEPSILONSMALL)
         {
            negbaNorm /= normMag;            

            normDP = negbaNorm.DotProductWith(originTobDir);
            if (MEHISCOARSERZERO(normDP))
               normDP = 0.0;
         }
         else
            normDP = 0.0;
         
         if (normDP >= 0.0)
         {                 
            if (aTobDotOriginTob >= 0.0)
            {
               // if we go into the "distance squared space" we can avoid some divide operations
               // we proceed by using barycentric coordinates with the barycenter at b
               // then we parameterize the triangle with:
               // T(s,t) = B + s*bToa + t*bToc
               // the distance squared formula from the origin to this triangle gives us:
               // Q(s,t) = a*s^2 + 2*b*s*t + c*t^2 + 2*d*s + 2*e*t + f, where:
               // a = bToa.bToa
               // b = bToa.bToc
               // c = bToc.bToc
               // d = bToa.originTob
               // e = bToc.originTob
               // f = originTob.originTob
               // we reduce the problem to one dimension by the following relation: t = 0 (in other words, we are looking for                     
               // the squared distance to the boundary of the triangle:
               // F(s) = Q(s,0) = a*s^2 + 2*d*s + f
               // differentiating and equating to zero for the critical point, we get:
               // F'(s) = a*s + d = 0
               // s = -d / a = aTob.originTob / aTob.aTob
               
               scalar s = aTobDotOriginTob / aTobDotaTob;
               outContactPoint = aTob;
               outContactPoint *= -s;
               outContactPoint += b;
               MEHASSERT(outContactPoint.Y() < MEHMAXSCALAR && outContactPoint.Y() > MEHMINSCALAR);
               simplexPoints.Remove(2);
               // we want barycentric coordinates such that no additional math needs to be done to calculate the closest points as 
               // follows:
               // T'(x,y,z) = A*x + B*y + C*z
               // T' = B + (A-B)s + (C-B)t = B + As - Bs + Ct - Bt = B*(1-s-t) + As + Ct
               simplexPoints[0].barycentricCoord = s;
               simplexPoints[1].barycentricCoord = (scalar)1.0 - s;
               break;
            }
         }
         
         // vert B
         if (cTob.DotProductWith(originTob) <= 0.0 && aTobDotOriginTob <= 0.0)
         {
            // b is closest point, remove the rest
            simplexPoints.Remove(2);
            simplexPoints.Remove(0);
            simplexPoints[0].barycentricCoord = 1.0;
            break;
         }
                  
         // edge CB
         MEHVector3 bcNorm = normal;
         bcNorm.ToCrossProductWith(cTob);
         normMag = bcNorm.Magnitude();
         
         cTobDotcTob = cTob.Magnitude2();
         cTobDotOriginToc = cTob.DotProductWith(originToc);
         
         if (normMag > MEHEPSILONSMALL)
         {
            bcNorm /= normMag;
                                    
            // make sure it's not in the interior of the triangle
            normDP = bcNorm.DotProductWith(originTobDir);
            if (MEHISCOARSERZERO(normDP))
               normDP = 0.0;
         }
         else
            normDP = (scalar)0.0;
         
         if (normDP <= 0.0)
         {  
            if (cTobDotOriginToc <= 0.0)
            {                                          
               // T(s,t) = C + s*cTob + t*cToa
               // the distance squared formula from the origin to this triangle gives us:
               // Q(s,t) = a*s^2 + 2*b*s*t + c*t^2 + 2*d*s + 2*e*t + f, where:
               // a = cTob.cTob
               // b = cTob.cToa
               // c = cToa.cToa
               // d = cTob.originToc
               // e = cToa.originToc
               // f = originToc.originToc
               // we reduce the problem to one dimension by the following relation: t = 0 (in other words, we are looking for                     
               // the squared distance to the boundary of the triangle:
               // F(s) = Q(s,0) = a*s^2 + 2*d*s + f
               // differentiating and equating to zero for the critical point, we get:
               // F'(s) = a*s + d = 0
               // s = -d / a = cTob.originToc / cTob.cTob                 
               
               scalar s = -cTobDotOriginToc / cTobDotcTob;
               outContactPoint = cTob;
               outContactPoint *= s;
               outContactPoint += c;
               MEHASSERT(outContactPoint.Y() < MEHMAXSCALAR && outContactPoint.Y() > MEHMINSCALAR);
               simplexPoints.Remove(0);
               // removed 0 (a), 0th element is c now, 1st element is b
               // we want barycentric coordinates such that no additional math needs to be done to calculate the closest points as 
               // follows:
               // T'(x,y,z) = A*x + B*y + C*z
               // T' = C + (B-C)s + (A-C)t = C + Bs - Cs + At - Ct = C*(1-s-t) + Bs + At
               simplexPoints[0].barycentricCoord = (scalar)1.0 - s;
               simplexPoints[1].barycentricCoord = s;
               break;
            }
         }
         
         // vert C             
         if (cToa.DotProductWith(originToc) >= 0.0 && cTobDotOriginToc >= 0.0)
         {
            // c is closest point, remove the rest
            outContactPoint = simplexPoints[2].minowskiPoint;
            simplexPoints.Remove(0);
            simplexPoints.Remove(1);
            simplexPoints[0].barycentricCoord = 1.0;
            break;
         }
         
         // edge AC         
         // we just need to see if it's in the interior or not
         MEHVector3 negacNorm = normal;
         negacNorm.ToCrossProductWith(cToa);
         normMag = negacNorm.Magnitude();
         
         if (normMag > MEHEPSILONSMALL)
         {
            negacNorm /= normMag;
                  
            MEHVector3 originTocDir = originToc;
            originTocDir.Normalize();
            
            normDP = negacNorm.DotProductWith(originTocDir);
            if (MEHISCOARSERZERO(normDP))
               normDP = 0.0;
         }
         else
            normDP = (scalar)0.0;
         
         if (normDP >= 0.0)
         {
            // T(s,t) = C + s*cTob + t*cToa
            // we reduce the problem to one dimension by the following relation: s = 0 (in other words, we are looking for
            // the squared distance to the boundary of the triangle:
            // F(t) = Q(0,t) = 2*e*t + c*t^2 + f
            // differentiating and equating to zero for the critical point, we get:
            // F'(t) = c*t + e = 0
            // t = -e / c = -cToa.originToc / cToa.cToa
            scalar t = -cToa.DotProductWith(originToc) / cToaDotcToa;
            outContactPoint = cToa;
            outContactPoint *= t;
            outContactPoint += c;
            MEHASSERT(outContactPoint.Y() < MEHMAXSCALAR && outContactPoint.Y() > MEHMINSCALAR);
            // remove b
            simplexPoints.Remove(1);
            // we want barycentric coordinates such that no additional math needs to be done to calculate the closest points as follows:
            // T'(x,y,z) = A*x + B*y + C*z
            // T' = C + (B-C)s + (A-C)t = C + Bs - Cs + At - Ct = C*(1-s-t) + Bs + At
            simplexPoints[0].barycentricCoord = t;
            simplexPoints[1].barycentricCoord = (scalar)1.0 - t;
            break;
         }
         
         // if we got this far, it is in the interior of the triangle
         // we go into the "distance squared space" as before
         // we proceed by using barycentric coordinates with the barycenter at b
         // then we parameterize the triangle with:
         // T(s,t) = B + s*bToa + t*bToc
         // the distance squared formula from the origin to this triangle gives us:
         // Q(s,t) = a*s^2 + 2*b*s*t + c*t^2 + 2*d*s + 2*e*t + f, where:
         // a = bToa.bToa
         // b = bToa.bToc
         // c = bToc.bToc
         // d = bToa.originTob
         // e = bToc.originTob
         // f = originTob.originTob
         // differentiating and equating to zero for the critical points, we get:
         // Qs'(s,t) = 2*a*s + 2*b*t + 2*d = 0
         // Qs'(s,t) = a*s + b*t = -d
         // Qt'(s,t) = 2*b*s + 2*c*t + 2*e = 0
         // Qt'(s,t) = b*s + c*t = -e
         
         // s = (-d - b*t) / a
         // b*(-d - b*t)/a + c*t = -e
         // -b*d/a - b*b*t/a + c*t = -e
         // -b*d - b*b*t + a*c*t = -e*a
         // (a*c - b*b)*t = b*d - e*a
         // t = (b*d - e*a) / (a*c - b*b), note that the denominator is the normal magnitude squared
         
         // t = (-e - b*s) / c
         // a*s + b*(-e - b*s)/c = -d
         // a*c*s - b*e - b*b*s = -c*d
         // s * (a*c - b*b) = -c*d + b*e
         // s = (b*e - c*d) / (a*c - b*b), note that the denominator is the normal magnitude squared
         
         // s = (bToa.bToc * bToc.originTob - bToc.bToc * bToa.originTob) / normal.normal
         normMag = normal.Magnitude2();
         if (normMag < MEHEPSILONSMALL)
            normMag = MEHEPSILONSMALL;
         scalar invNormMagSquared = (scalar)1.0 / normMag;
         scalar cTobDotOriginTob = cTob.DotProductWith(originTob);
         scalar aTobDotcTob = aTob.DotProductWith(cTob);
         scalar negs = (aTobDotcTob * cTobDotOriginTob - cTobDotcTob * aTobDotOriginTob) * invNormMagSquared;
         
         // t = (bToa.bToc * bToa.originTob - bToc.originTob * bToa.bToa) / normal.normal
         scalar negt = (aTobDotcTob * aTobDotOriginTob - cTobDotOriginTob * aTobDotaTob) * invNormMagSquared;

         MEHVector3 tTerm = cTob;
         tTerm *= negt;
         
         outContactPoint = aTob;
         outContactPoint *= negs;
         outContactPoint += tTerm;
         outContactPoint += b;
         MEHASSERT(outContactPoint.Y() < MEHMAXSCALAR && outContactPoint.Y() > MEHMINSCALAR);
         
         // we want barycentric coordinates such that no additional math needs to be done to calculate the closest points as follows:
         // T'(x,y,z) = A*x + B*y + C*z
         // T' = B + (A-B)s + (C-B)t = B - Bs + As + Ct - Bt = B*(1-s-t) + As + Ct
                  
         simplexPoints[0].barycentricCoord = -negs;
         simplexPoints[1].barycentricCoord = (scalar)1.0 + negs + negt;
         simplexPoints[2].barycentricCoord = -negt;
      }
      break;
      
      case 4:
      {
         MEHVector3 &a = simplexPoints[0].minowskiPoint;
         MEHVector3 &b = simplexPoints[1].minowskiPoint;
         MEHVector3 &c = simplexPoints[2].minowskiPoint;
         MEHVector3 &d = simplexPoints[3].minowskiPoint;
         MEHArray<MEHGJKPoint, 4> triangles[4];
         
         scalar closestDistSquared = MEHMAXSCALAR;
         s32 closestIdx = -1;
         
         MEHVector3 vec1 = c;
         vec1 -= b;
         MEHVector3 vec2 = a;
         vec2 -= b;
         MEHVector4 plane = vec1;
         plane.ToCrossProductWith(vec2);
         ((MEHVector3 *)&plane)->Normalize();
         scalar D1 = -b.DotProductWith(plane);
         plane.SetW(D1);
                           
         scalar &originDist1 = D1;
         scalar otherSideDist1 = plane.DotProductWith(d);
         if (otherSideDist1 < MEHEPSILONLARGE)
            otherSideDist1 = 0.0;
         
         if (originDist1 * otherSideDist1 <= MEHEPSILONLARGE)
         {            
            *triangles[0].GetFreePlace() = simplexPoints[0];
            *triangles[0].GetFreePlace() = simplexPoints[1];
            *triangles[0].GetFreePlace() = simplexPoints[2];
            FindClosestPointInSimplex(triangles[0], outContactPoint);
            closestDistSquared = outContactPoint.Magnitude2();
            closestIdx = 0;
         }
         
         MEHVector3 vec3 = d;
         vec3 -= b;
         plane = vec2;
         plane.ToCrossProductWith(vec3);
         ((MEHVector3 *)&plane)->Normalize();
         scalar D2 = -b.DotProductWith(plane);
         plane.SetW(D2);
                           
         scalar &originDist2 = D2;
         scalar otherSideDist2 = plane.DotProductWith(c);
         if (otherSideDist2 < MEHEPSILONLARGE)
            otherSideDist2 = 0.0;
         
         if (originDist2 * otherSideDist2 <= MEHEPSILONLARGE)
         {
            *triangles[1].GetFreePlace() = simplexPoints[0];
            *triangles[1].GetFreePlace() = simplexPoints[1];
            *triangles[1].GetFreePlace() = simplexPoints[3];
            MEHVector3 triPt;          
            FindClosestPointInSimplex(triangles[1], triPt);
            scalar distSquared = triPt.Magnitude2();
            
            if (distSquared < closestDistSquared)
            {
               outContactPoint = triPt;
               closestDistSquared = distSquared;
               closestIdx = 1;
            }
         }
         
         plane = vec3;
         plane.ToCrossProductWith(vec1);
         ((MEHVector3 *)&plane)->Normalize();
         scalar D3 = -b.DotProductWith(plane);
         plane.SetW(D3);
                           
         scalar &originDist3 = D3;
         scalar otherSideDist3 = plane.DotProductWith(a);
         if (otherSideDist3 < MEHEPSILONLARGE)
            otherSideDist3 = 0.0;
         
         if (originDist3 * otherSideDist3 <= MEHEPSILONLARGE)
         {
            *triangles[2].GetFreePlace() = simplexPoints[2];
            *triangles[2].GetFreePlace() = simplexPoints[1];
            *triangles[2].GetFreePlace() = simplexPoints[3];
            MEHVector3 triPt;
            FindClosestPointInSimplex(triangles[2], triPt);
            scalar distSquared = triPt.Magnitude2();
            
            if (distSquared < closestDistSquared)
            {
               outContactPoint = triPt;
               closestDistSquared = distSquared;
               closestIdx = 2;
            }
         }
         
         vec1 = a;
         vec1 -= c;
         plane = d;
         plane -= c;
         plane.ToCrossProductWith(vec1);
         ((MEHVector3 *)&plane)->Normalize();
         scalar D4 = -c.DotProductWith(plane);
         plane.SetW(D4);
                           
         scalar &originDist4 = D4;
         scalar otherSideDist4 = plane.DotProductWith(b);
         if (otherSideDist4 < MEHEPSILONLARGE)
            otherSideDist4 = 0.0;
         
         if (originDist4 * otherSideDist4 <= MEHEPSILONLARGE)
         {
            *triangles[3].GetFreePlace() = simplexPoints[0];
            *triangles[3].GetFreePlace() = simplexPoints[2];
            *triangles[3].GetFreePlace() = simplexPoints[3];
            MEHVector3 triPt;
            FindClosestPointInSimplex(triangles[3], triPt);
            scalar distSquared = triPt.Magnitude2();
            
            if (distSquared < closestDistSquared)
            {
               outContactPoint = triPt;
               closestDistSquared = distSquared;
               closestIdx = 3;
            }
         }
         
         if (closestIdx >= 0)
            simplexPoints = triangles[closestIdx];
      }
      break;
   }
}
#pragma warning(default: 4700)

HTH,
Mike

carlschissler · **Posted:** Fri Jul 06, 2012 8:23 am

I've actually fixed my problems for the most part, but thanks. Though, looking over your code it looks like you just take the GJK termination simplex and compute a contact point from that. I'm not sure if that will work for shapes that are more complex than boxes if there is much penetration - I used to just find the closest point to the origin on the simplex but that produces 'squishy' objects that don't recover from penetration because the penetration depth isn't calculated correctly. Hence the need for the Expanding Polytope Algorithm to determine the actual penetration depth and points.

I hadn't realized until today just how hard it is to program robust implementations of these algorithms. In the process of fixing my original bug I managed to find and fix at least 5 other subtle errors! I still get a few degenerate EPA triangles every 20-30 seconds in huge complex scenes, but I just terminate the algorithm and use the next closest triangle to the minkowski origin in those cases and it works well.

Physics Simulation Forum

My EPA doesn't work for spheres + capsules, why?

Who is online