EPA (Expanding Polytope Algorithm)

in Collision Detection, Game Development

In the last few posts we learned about using GJK for collision detection, distance between shapes, and finding the closest points. It was stated that GJK must be augmented, to find collision information like the penetration depth and vector, with another algorithm. One such algorithm is EPA.

I plan to cover the EPA algorithm and mention an alternative.

  1. Introduction
  2. Overview
  3. Starting Point
  4. Expansion
  5. Example
  6. Winding and Triple Product
  7. Augmenting
  8. Alternatives
Figure 1: Two convex shapes intersecting

Introduction
Like GJK, EPA uses the same concept of a Minkowski Sum, performing the difference operation instead of addition. Like the previous posts I will refer to this as the Minkowski Difference.

In the first GJK post we talked about how to determine if two convex shapes were intersecting; true or false. What we want to do now is after we determine that there is a collision, find the collision information: depth and vector.

Overview
In the GJK post we stated that we know the convex shapes are intersecting if the Minkowski Difference contains the origin. In addition to this, the distance from closest point on the Minkowski Difference to the origin is the penetration depth. Likewise, the vector from the closest point to the origin is the penetration vector.

Figure 2: The Minkowski Difference

Like GJK, EPA is an iterative algorithm. EPA stands for Expanding Polytope Algorithm and means just that. We want to create a polytope (or polygon) inside of the Minkowski Difference and iteratively expand it until we hit the edge of the Minkowski Difference. The key is to expand the closest feature to the origin of the polytope. If we perform this iteratively we will generate a polytope where the closest feature lies on the Minkowski Difference, thereby yeilding the penetration depth and vector.

EPA performs this task by using the same support function used in the other algorithms and the same notion of a simplex. One difference from GJK is that EPA’s simplex can have any number of points.

Starting Point
EPA needs an initial simplex to expand. The terminiation simplex from the GJK collision detection routine is a great start.

EPA needs a full simplex: Triangle for 2D and Tetrahedron for 3D. The GJK collision detection routine can be modified such that it always terminiates with the above cases. The GJK post will never terminiate, returning that the shapes are intersecting, until a triangle has been created.

Expansion
If we pass the termination simplex to EPA we can immediately start the expansion process:


Simplex s = // termination simplex from GJK
// loop to find the collision information
while (true) {
  // obtain the feature (edge for 2D) closest to the origin on the Minkowski Difference
  Edge e = findClosestEdge(s);
  // obtain a new support point in the direction of the edge normal
  Vector p = support(A, B, e.normal);
  // check the distance from the origin to the edge against the
  // distance p is along e.normal
  double d = p.dot(e.normal);
  if (d - e.distance < TOLERANCE) {
    // the tolerance should be something positive close to zero (ex. 0.00001)
  
    // if the difference is less than the tolerance then we can
    // assume that we cannot expand the simplex any further and
    // we have our solution
    normal = e.normal;
    depth = d;
  } else {
    // we haven't reached the edge of the Minkowski Difference
    // so continue expanding by adding the new point to the simplex
    // in between the points that made the closest edge
    simplex.insert(p, e.index);
  }
}

Where the findClosestEdge looks something like:


Edge closest = new Edge();
// prime the distance of the edge to the max
closest.distance = Double.MAX_VALUE;
// s is the passed in simplex
for (int i = 0; i < s.length; i++) {
  // compute the next points index
  int j = i + 1 == s.length ? 0 : i + 1;
  // get the current point and the next one
  Vector a = s.get(i);
  Vector b = s.get(j);
  // create the edge vector
  Vector e = b.subtract(a); // or a.to(b);
  // get the vector from the origin to a
  Vector oa = a; // or a - ORIGIN
  // get the vector from the edge towards the origin
  Vector n = Vector.tripleProduct(e, oa, e);
  // normalize the vector
  n.normalize();
  // calculate the distance from the origin to the edge
  double d = n.dot(a); // could use b or a here
  // check the distance against the other distances
  if (d < closest.distance) {
    // if this edge is closer then use it
    closest.distance = d;
    closest.normal = n;
    closest.index = j;
  }
}
// return the closest edge we found
return closest;

Figure 3: GJK Termination Simplex

Example
As always, I think its much easier to understand once you go through an example. We will use the example from the GJK post and determine the collision information using EPA.

We start by supplying the GJK termination simplex to EPA:

Iteration 1

Simplex s = {(4, 2), (-8, -2), (-1, -2)};
Edge closest = null;
{ // compute the closest edge
  // Edge 1
  a = (4, 2), b = (-8, -2)
  e = b - a = (-12, -4)
  oa = a = (4, 2)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (4, 2) * 160 - (-12, -4) * -56 = (640, 320) + (-672, -224) = (-32, 96)
  // normalize
  n ≈ (-32 / 101.19, 96 / 101.19) ≈ (-0.32, 0.95)
  d = a.dot(n) = 4 * -0.32 + 2 * 0.95 ≈ 0.62;
  
  // Edge 2
  a = (-8, -2), b = (-1, -2)
  e = b - a = (7, 0)
  oa = a = (-8, -2)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (-8, -2) * 49 - (7, 0) * -56 = (-392, -98) + (392, 0) = (0, -98)
  // normalize
  n = (0 / 98, -98 / 98) = (0, -1)
  d = a.dot(n) = -8 * 0 + -2 * -1 = 2;
  
  // Edge 3
  a = (-1, -2), b = (4, 2)
  e = b - a = (5, 4)
  oa = a = (-1, -2)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (-1, -2) * 41 - (5, 4) * -13 = (-41, -82) - (-65, -52) = (24, -30)
  // normalize
  n ≈ (24 / 38.42, -30 / 38.42) ≈ (0.62, -0.78)
  d = a.dot(n) = -1 * 0.62 + -2 * -0.78 ≈ 0.94;
  
  // we can see that Edge 1 is the closest, so...
  closest.normal = (-0.32, 0.95)
  closest.distance ≈ 0.62
  closest.index = 1;
}

// get a new support point in the direction of closest.normal
p = support(A, B, closest.normal) = (4, 11) - (10, 2) = (-6, 9)

// are we close enough?
dist = p.dot(closest.normal) = -6 * -0.32 + 9 * 0.95 = 1.92 + 8.55 = 10.47
// 10.47 - 0.62 = 9.85 obviously too big

// add it to the simplex at the index
s.add(p, closest.index)
// which makes s = {(4, 2), (-6, 9), (-8, -2), (-1, -2)};
Figure 4: Post EPA Iteration 1 Simplex

Notice that we have expanded the simplex by adding another point. Its important to point out that the point we added is on the edge of the Minkowski Difference. Because all points that make up the simplex lie on the edge Minkowski Difference we can guarentee that the simplex is convex since the Minkowski Difference is convex. If the simplex was not convex then we wouldn’t be able to skip so many computations.

Another key that should be pointed out is how we add the new point to the simplex. We must add the new point in between the two points that create the closest edge. This way the shape stays convex. In this example the winding of the points doesn’t matter, however it is important to notice that insert the new point as we have done preserves the current winding direction. More on the simplex’s winding direction later…

Iteration 2

Simplex s = {(4, 2), (-6, 9), (-8, -2), (-1, -2)};
Edge closest = null;
{ // compute the closest edge
  // Edge 1
  a = (4, 2), b = (-6, 9)
  e = b - a = (-10, 7)
  oa = a = (4, 2)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (4, 2) * 149 - (-10, 7) * -26 = (596, 298) + (-260, 182) = (336, 480)
  // normalize
  n ≈ (336 / 585.91, 480 / 585.91) ≈ (0.57, 0.82)
  d = a.dot(n) = 4 * 0.57 + 2 * 0.82 ≈ 3.92;
  
  // Edge 2
  a = (-6, 9), b = (-8, -2)
  e = b - a = (-2, -11)
  oa = a = (-6, 9)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (-6, 9) * 125 - (-2, -11) * -87 = (-750, 1125) + (-174, -957) = (-924, 168)
  // normalize
  n ≈ (-924 / 939.15, 168 / 939.15) ≈ (-0.98, 0.18)
  d = a.dot(n) = -6 * -0.98 + 9 * 0.18 ≈ 7.5;
  
  // Edge 3
  a = (-8, -2), b = (-1, -2)
  e = b - a = (7, 0)
  oa = a = (-8, -2)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (-8, -2) * 49 - (7, 0) * -56 = (-392, -98) + (392, 0) = (0, -98)
  // normalize
  n = (0 / 98, -98 / 98) = (0, -1)
  d = a.dot(n) = -8 * 0 + -2 * -1 = 2;
  
  // Edge 4
  a = (-1, -2), b = (4, 2)
  e = b - a = (5, 4)
  oa = a = (-1, -2)
  // triple product (A x B) x C = B(A.dot(C)) - A(B.dot(C))
  // oa(e.dot(e)) - e(oa.dot(e))
  n = (-1, -2) * 41 - (5, 4) * -13 = (-41, -82) - (-65, -52) = (24, -30)
  // normalize
  n ≈ (24 / 38.42, -30 / 38.42) ≈ (0.62, -0.78)
  d = a.dot(n) = -1 * 0.62 + -2 * -0.78 ≈ 0.94;
  
  // we can see that Edge 4 is the closest, so...
  closest.normal = (0.62, -0.78)
  closest.distance ≈ 0.94
  closest.index = 0;
}

// get a new support point in the direction of closest.normal
p = support(A, B, closest.normal) = (9, 9) - (5, 7) = (4, 2)

// are we close enough?
dist = p.dot(closest.normal) = 4 * 0.62 + 2 * -0.78 = 0.92
// 0.92 - 0.94 = 0.02 small enough!
// we exit the loop returning (0.62, -0.78) as the collision normal
// and 0.92 as the depth

In the second iteration we see that the closest edge of the simplex actually lies on the Minkowski Difference. We can see by inspection that Edge 4′s normal is the collision normal and that the perpendicular distance from the edge to the origin is the penetration depth. This is confirmed in the last iteration.

We terminated on the second iteration because the distance to the new simplex point was not more than the distance to the closest edge indicating that we cannot expand our simplex any futher. If higher precision numbers were used we would see that the value of the dist variable would be much closer to zero which makes sense since the new support point lies on the closest edge.

We still need to have a tolerance because of curved shapes and finite precision math. For a curved Minkowski Difference, the simplex will build smaller and smaller edges to conform to the curvature. You can see this in figure 5, however it may be many more edges because of the increase precision.

Figure 5: Example Curved Minkowski Difference

Winding and Triple Product
Earlier I mentioned something about the winding of the simplex being preserved. This is important to handle a special case of collisions.

Small or touching collisions can cause EPA problems which stem from the use of the triple product. If the origin lies really close to the closest edge the triple product may return a zero vector (because of finite precision). When we go to normalize the vector we will divide by zero: not good.

If we look at the reason we are using the triple product we come up with another method to fix this problem. The triple product is used to get the normal vector of an edge in the opposite direction of the origin. We can do the same thing by using the per-product of the edge.

The per-product is defined as:

if A = (x, y)
A.perproduct() = (-y, x) or (y, -x) depending on the
handedness of the coordinate system (right or left respectively)

In this instance the left or right handedness actually is determined by the winding of the simplex. If the winding of the simplex is counter-clockwise then we want to use the right per-product. Likewise if the simplex winding is clockwise then we want to use the left per-product. We can assume this because we have already guarenteed that the origin is contained in the simplex.

So instead of using the triple product we can use the per-product to get the normal of the edge no matter how close the origin is to the closest edge. The new code looks something like this:


// we change this
// Vector n = Vector.tripleProduct(e, oa, e);
// to this
if (winding == CLOCKWISE) {
  n = e.left(); // (y, -x)
} else {
  n = e.right(); // (-y, x)
}

It was important to note that the winding of the simplex is preserved because this means we can determine the winding of the simplex once making the new code more efficient and robust.

Augmenting
EPA is often not used for small penetrations because of the computational cost. Therefore you may see EPA supplemented with GJK penetration detection algorithm. To use GJK for collision information involves using smaller versions of the colliding shapes (called core shapes) and performing a GJK distance check. Once the distance between the core shapes is found, subtract it from the sum of the radial shinkage applied to the shapes.

Figure 6: Example “Core Shapes”

Alternatives
There are alternatives to using EPA to determine collision information after GJK has detected a collision. I’m only going to mention one here: sampling for the smallest penetration.

Generate a sample of directions. Find the distance from the origin to the surface of the Minkowski Difference (like we do in EPA) along each direction. Use the one with the smallest distance.

This is obviously an estimation instead of an exact answer, however it can be improved by intelligent selection of the directions.

  • Evenly distribute the directions along the unit circle/sphere
  • Use more directions
  • Use the edge normals of each shape
  • Use the vector from center to center of the shapes
  • etc.

For low vertex count polygons this may seem like more work, but for high vertex count polyhedrons it can be faster than EPA and still yield an acceptable result.

The above EPA implementation adds 22+ operations every time a new simplex point is added. This is significant when the Minkowski Difference has many vertices or has curved edges.

22 Comments

22 Comments

  1. normal = edge.normal;
    or normal = e.normal?

  2. Thanks again for finding that mistake. I fixed it within the post.

    William

  3. Hi!

    I really like your walkthrough of the EPA algorithm! But I have a question.
    You talk a lot about the winding of the simplex, and I do understand how that would work in 2D, How would you do in 3d, as I am about to implement this in to my 3d game engine that already got GJK working nicely, it would be very practical if I knew this.

    Greetings from Sweden!
    Mr.Ful

  4. Very good question. It’s possible that you may obtain the zero vector in 3D also. I have not implemented the algorithm in 3D. However, EPA should be basically the same, instead of using edge normals, you use face normals. A face (which can be thought of as a plane) has two possible normals, one pointing outside of the shape, and one pointing inside. Getting one or the other depends on the winding of the face vertices.

    You can attempt to solve the problem by forcing all faces to have the same winding (counter-clockwise to obtain the face normal pointing outside the shape) or you can save the normals with the shape itself with some reference to what face it is for.

    If I were you, I would implement EPA without worrying about the zero vector problem. This only crops up when shapes are touching. Once you get it working for the majority of the cases then you can try to fix the outliers.

    Another way to solve the problem is avoid using EPA for the “small penetration” cases. This is briefly explained in the Augmenting section of the post (see figure 6). This approach is probably much faster and more stable for 3D.

    William

  5. weijin

    I don’t understand how to get contact point from GJK and EPA.
    Could you please give me some instructions

    Thanks

  6. William

    Good question, and the distinction is good to note here. The post only covered how to obtain the contact normal and depth, not the contact points. In fact, finding the contact points from the contact normal and depth is a entirely different subject in itself. I do not have a post on this yet.

    The dyn4j project uses a modified clipping method (handling circle shapes is the only difference really) that is used in box2d to obtain the contact points from the contact normal and depth.

    If you choose to go the “Core Shapes” route then you may be able to get this information by using the GJK distance algorithm. The GJK distance algorithm can be used on the “Core Shapes” to obtain the closest points and separation vector (see the GJK – Distance & Closest Points post). Then you can translate these points along the separation vector depth distance to obtain a collision point on each shape (you probably should only use one of them). I haven’t thought it through completely, but this may or may not be accurate. I want to say that bullet is doing this for shallow penetration, but I don’t remember. There are more details here, but this is probably enough to get you going for now.

  7. Hello,
    First of all I want to say your guides are really good!

    I know that they asked this already, but I can found the contact points of the collision if I project the vertex which I got from the distance algorithem onto the edge of the second shanpe?

    If i have spelling mistakes, it’s because English isn’t my regular language :P
    thank you!

  8. No problem. If you use core shapes, ie the shapes used to detect collision are smaller than the real shapes, then you can use the closest points from the GJK distance method. I haven’t researched the method enough to know for sure how to obtain the collision points from the closest points, but check out this PDF pages 40-44 it talks about using a collision margin and the GJK distance method to find the penetration depth and contacts.

    In addition, Bullet uses something called contact caching to get enough points for stable stacking as well. As you can see in the PDF the distance method will typically give you just one contact point. So Bullet stores the contact point, waits until the next iteration of the engine to get another point, and so on until it has enough points to solve the collision. As new points are added, a reduction algorithm is used to only keep the points that are “best,” which is usually the deepest penetration points, or the points whose area is the largest, etc.

    In Box2d and dyn4j, the contacts for each collision are found in one step instead of caching until enough points are available. This is done by a feature clipping method that really requires its own post to explain well.

    William

  9. I am also curious about how to detect winding of a simplex.

  10. William

    The winding of the simplex can be found just like you would find the winding of any polygon. See this for more details.

    As a performance enhancement, you only really need to know the value of the first non-zero cross product to determine the winding. In addition, since EPA retains the winding of the simplex throughout the algorithm, you only need to calculate the winding when the algorithm starts.

    William

  11. Quentin

    Hi and first of all thank you four your guides ! It is really the best I found on the subject (GJK & EPA).

    I have yet a little problem:
    I have a particular case where the origine belongs to the edge formed by the two first point of my simplex (in GJK). In this case the algorithm find that the shape are not colliding even if they are…
    I tried to fix this by returning a colision if the origin belongs to an edge of the simplex but then I have a problem in the EPA who seems unable to find the closest edge (infinite loop).

    English is not my first language so I’m not sure if I ‘m clear enough.

    Here is my example :

    Shape 1 points: (2,2),(2,5),(4,5),(4,2)
    Shape 2 points: (3,3),(5,4),(5,2)
    First d : center shape 1 – center shape 2
    First edge in my simplex formed by :(-3,3), (1,-1)
    epsilon chosen: 0.01

  12. Quentin

    Little precision: When I fix this in GJK the output simplex only have 2 points and when I look for the closest edge the on I find has a e.distance =0 . So I never enter the if(d – e.distance < epsilon)…

  13. William

    Yeah these are some of the cases where GJK and EPA can get tricky. First let me say that the EPA guide here requires that the final simplex from GJK is a triangle (for 2D) to work correctly.

    There are two sub cases for the case in which the origin is on an edge of the simplex:
    1. The edge of the simplex (which the origin lies on) is an edge of the Minkowski Sum
    2. The edge of the simplex (which the origin lies on) lies inside of the Minkowski Sum

    In the first case, this indicates touching contact between the two shapes.
    The second case is a bit of a problem in 3D for the line segment case.

    Thankfully in 2D we have an easy way out for both of these cases. If the origin lies on an edge of the simplex we can just use either perpendicular vector of that edge as the next search direction (the modified line segment case of GJK.containsOrigin):

    
    // then its the line segment case
    b = Simplex.getB();
    // compute AB
    ab = b - a;
    // get the perp to AB in the direction of the origin
    abPerp = tripleProduct(ab, ao, ab);
    // is the new direction vector close to the zero vector?
    if (abPerp.getSquaredMagnitude() <= EPSILON) {
      d.set(ab.left());
      // you can use either v.left() = (v.y, -v.x)
      // or v.right() = (-v.y, v.x)
    } else {
      // set the direction to abPerp
      d.set(abPerp);
    }
    
    

    William

  14. Quentin

    Thank you so much for this fast and accurate answer William !

    I’m in 2D(second case) and it works perfectly !

    Just a little thing: in your correction I believe it is ab.left and not abPerp.left as abPerp is “null”.

    Thanks a lot !

    Quentin

  15. William

    Awesome! Yeah, I meant to put ab.left() instead of abPerp. I fixed it in the previous comment of mine. Thanks for pointing that out!

    William

  16. honestann

    After a collision, why not separate objects by moving them back along their velocity vectors? This should get the objects back to their actual positions at the TOI (time of impact), which is clearly more precise.

  17. Sure, but how do you know how much to move them without first knowing how much they are penetrating? In addition, what happens when two bodies at rest (both have zero velocity) and are overlapping?

    Can you elaborate a little more on what you are describing?

    William

  18. when you calculate the distance from origin to the edge shouldn’t a or normal( n ) be in opposite direction?

  19. @ng

    Can you point to the place in the post where you think it should be in the opposite direction? More specifically, which iteration and which edge calculation?

    I am making some assumptions that may not be apparent that may answer your question:
    1. I’m using a right handed coordinate system
    2. When computing the vector from A to B, you subtract the vectors in reverse order: A to B becomes B – A.

    William

Leave a Reply

Using Gravatars in the comments - get your own and be recognized!

XHTML: These are some of the tags you can use: <a href=""> <b> <blockquote> <code> <em> <i> <strike> <strong>