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.

- Introduction
- Overview
- Starting Point
- Expansion
- Example
- Winding and Triple Product
- Augmenting
- Alternatives

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.

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:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 |
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:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 |
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; |

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:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 |
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)}; |

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 |
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.

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:

1 2 3 |
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:

1 2 3 4 5 6 7 8 |
// 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.

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.

normal = edge.normal;

or normal = e.normal?

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

William

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

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

withoutworrying 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

I don’t understand how to get contact point from GJK and EPA.

Could you please give me some instructions

Thanks

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.

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!

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

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

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

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

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)…

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):

William

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

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

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.

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

If Polygon A = [(0, 0), (0, 4), (4, 4), (4, 0)]

and Polygon B = [(2, 3), (2, 7), (6, 7), (6, 3)]

then EPA will give a penetration depth of 1 and a vector of (0, 1)

but if A’s velocity was (2, 0) and B’s velocity was (-2, 0), the polygons’ ys shouldn’t change at all, so how is the info from EPA supposed to help?

@Harry

Right, that wouldn’t be the correct normal even though that’s what EPA would find. This is because the objective of the algorithm. The algorithm’s objective is NOT to find the penetration normal of two moving convex shapes, but rather to find the

minimumpenetration normal of the two shapes. Clearly, these don’t mean the same thing and its obvious from your example.Now with that said, you wouldn’t typically

createyour shapes overlapping in that manner and hopefully your simulation would run fast enough that a penetration of that sort wouldn’t happen.William

Do you know of any algorithms that get the penetration normal for moving shapes?

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

@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

Hello, and thank you for this awesome article!!

Well, I had a same question with @ng, but I’ve understood it when I write my question :D

Could you please modify your comment at line 15 of findClosestEdge (“get the vector from the edge towards the origin”). I think it got me and @ng mixed up.

Thanks a lot!!

Hi!

I have a little problem with the 3D EPA algorithm. Considering a box-box collision, there are cases when the algorithm doesn’t terminate ever: the polytope is just getting bigger with redundant vertices.

Is there any way to detect this? (besides limiting the number of iterations/vertices)

@Asylum

EPA should complete in a finite number of iterations for polytopes, although it could run forever on curved solids. Have you been able to get a reproducible test case that you can debug through? Have you looked through my 2D implementation here?

William

Hi!

Yes, I’m debugging it at the moment. The test case is two 1×1 boxes sitting exactly on eachother.

Initial simplex is:

0: (0, 0, 0)

1: (0, 1, 1)

2: (-1, 1, 0)

3: (-1, 0, 1)

The algorithm finds the first face to be the best (0, 1, 2). Normal vector is (-1, -1, 1), therefore the support vertex obtained in this direction will be vertex 4 -> redundancy.

I added a condition, so in the next iteration the algorithm will detect the infinite loop (it didn’t get closer to the origin), however the terminating triangle is not on the surface of the CSO -> contact normal will be bad.

I take a look at your code.

Hi!

I am a little confused:

1. EPA starts with a edge of Minkowski Difference(MD) so why simplex from GJK is needed?

2. In this example, the simplex is composed by two edges of MD so EPA tries to reach another edge of MD?

Btw, your tutorials are awesome! Thanks!

@Student

1. EPA doesn’t have to start with the simplex from GJK. It could build its own, but to save time you might as well just use the one from GJK. If implemented appropriately, GJK in 2D will always end with a triangle simplex.

2. The example here isn’t the best, since the closest edge is already part of the simplex. Generally, this may not be the case. As a result, EPA expands the simplex until it finds the edge closest to the origin.

William

Im doing epa in 3d and this is my code:

When I get to the part where Im crossing direction I dont understand what I should be doing because i dont know what your triple product was. Can this code even work?

@Evan

The difference for 3d is that you expand the simplex until you obtain the face, rather than an edge, that is closest to the origin.

For each face in the simplex, we need to compute the face normal. Doing so allows us to compute the distance to the origin via the dot product.

For example, if A, B and C define the vertices of a 3d triangle, we can get a normal by taking the cross product of any two edges: N = AB X AC.

Note that there are two normals here one that will point inward and one that will point outward.Using the dot product we can get the distance to the origin.The triple product in the code here is a way to get the normal of the edge that points in the direction of the origin without doing a dot product with AO. To keep things simple, I’d stick with the dot product for now.

Loop over all faces and obtain the closest one. Once the closest is found, get a new support point in that direction. If its no closer than the face, then you are done. If it is closer, then you must split that face into 3 faces using the new support point.

William

Since Vector a in findClosestEdge will change each loop, how does comparing Vector n with it (with the dot product) give an accurate comparison?

@Harry

I’m not comparing

nwitha, I’m comparingdwithclosest.distance. This compares the distance of current edge with the distance of the closest edge I’ve found so far.The dot product is a projection of the vector a onto the vector n. This gives us the

perpendiculardistance from the edge to the origin, which will be different for every edge.William