EPA: Collision response algorithm for 2D/3D

Last time we looked at an algorithm for testing collisions between two convex polygons called GJK. It's a useful algorithm for sure, but it doesn't give us enough information to respond to the collisions it detects. In this article I'll describe an extension that allows us to find the correct normal and depth of the collisions. This extension provides the information that the solvers need to resolve a collision as I demonstrated in my original physics engine article.

I call this algorithm an extension because its input is the internal state from GJK. If you recall, we were working with a simplex to try and surround the origin with a triangle in 2D or a tetrahedron in 3D. This algorithm takes the final simplex that contained the origin and finds the normal of collision, aka the shortest vector to nudge the shapes out of each other. The naive solution is to use the normal of the closest face to the origin, but remember, a simplex does not need to contain any of the original polygon's faces, so we could end up with an incorrect normal.

Let's jump into it, it's called the Expanding Polytope Algorithm because it expands the simplex by adding vertices to it until we find the shortest normal from a face that is on the original polygon. For jargon's sake, a simplex becomes a polytope after we begin adding more vertices to it. How do we do that? Well we already have the tools from GJK, all we need to do is iterate over the faces and find the closest one. Then we'll use the Support function to find if there is a point further in the direction of that normal. If there is one, the face of the polytope must be inside the original polygon and can be expanded. If so, we'll add this supporting point and repeat.

In 2D this is easy, we can just add the point and the edges remain intact. In 3D we need to do a little repair job, which is where most of the annoyance comes from. Let's start with the 2D version first, because it shows the algorithm's main pieces, then we'll see how to repair the 3D polytope.

2D in Javascript #

I'm going to venture into a little p5.js for the 2D version because I've made a small demo with it that you can check out in fullscreen here.

We'll start with a function that takes in the simplex from GJK and the two shapes that we are testing.

The first step is to find the edge closest to the origin. We'll do this by comparing the dot products between their normals and the vectors from the origin. We only need the closest one, so we'll store it, along with its index and distance in variables, outside the loop to use later.

function EPA(polytope, shapeA, shapeB) {
	let minIndex = 0;
	let minDistance = Infinity;
	let minNormal;

	while (minDistance == Infinity) {
		for (let i = 0; i < polytope.length; i++) {
			let j = (i + 1) % polytope.length;

			let vertexI = polytope[i].copy();
			let vertexJ = polytope[j].copy();

			let ij = vertexJ.sub(vertexI);

			let normal = createVector(ij.y, -ij.x).normalize();
			let distance = normal.dot(vertexI);

			if (distance < 0) {
				distance *= -1;
				normal.mult(-1);
			}

			if (distance < minDistance) {
				minDistance = distance;
				minNormal = normal;
				minIndex = j;
			}
		}

Finding the normal of a vector in 2D is done by swapping the X and Y components and flipping one of their signs. The handedness, either right or left, depends on which component gets flipped. Depending on the winding order, the handedness that results in an outward facing normal changes, so it's common to add a check to flip it if it's wrong.

Once we have this normal, we can use the Support function to test for a point further out in its direction. If there is one, we'll insert the support point between the vertices in the polytope and repeat. If there isn't one, we know we have found the actual closest normal and can return it. Adding a small amount to the distance is useful to stop repeat collisions that would have moved the shapes an even smaller distance.

		let support = support(shapeA, shapeB, minNormal);
		let sDistance = minNormal.dot(support);

		if (abs(sDistance - minDistance) > 0.001) {
		 	minDistance = Infinity;
			polytope.splice(minIndex, 0, support);
		}
	}

	return minNormal.mult(minDistance + 0.001);
}

That's it for the 2D version. Now that we've looked at the core of this algorithm, we can make the jump to 3D. The main difference between the two is that in 2D the edges are implied by the order of the vertices, but in 3D we need to explicitly define the faces because each vertex is shared between three or more faces.

3D in C++ #

CollisionPoints EPA(
	const Simplex& simplex, 
	const Collider& colliderA, 
	const Collider& colliderB)
{

In 3D we need to keep track of every face. I've seen some different techniques, but the most straightforward in my opinion is to treat it like a mesh and use an index. We'll start with the polytope like we did in the 2D case, but now we'll also store the faces in a separate list. The most common way to do this is to treat every three indices as a triangle.

This index makes it easy to calculate the normal once per face, instead of with every iteration like in the 2D version, so let's split that into its own function so we can calculate them before we start.

	std::vector<vec3> polytope(simplex.begin(), simplex.end());
	std::vector<size_t> faces = {
		0, 1, 2,
		0, 3, 1,
		0, 2, 3,
		1, 3, 2
	};

	// list: vec4(normal, distance), index: min distance
	auto [normals, minFace] = GetFaceNormals(polytope, faces);

We'll add the same main loop from before.

	vec3  minNormal;
	float minDistance = FLT_MAX;
	
	while (minDistance == FLT_MAX) {
		minNormal   = normals[minFace].xyz();
		minDistance = normals[minFace].w;
 
		vec3 support = Support(colliderA, colliderB, minNormal);
		float sDistance = dot(minNormal, support);
 
		if (abs(sDistance - minDistance) > 0.001f) {
			minDistance = FLT_MAX;

When expanding the polytope in 3D, we cannot just add a vertex, we need to repair the faces as well. You would think that removing the face and adding three more would be enough, but when two faces result in the same support point being added, duplicate faces end up inside the polytope and cause incorrect results. The solution is to remove not just the current face, but every face that is pointing in the direction of the support point. To repair it afterwards, we'll keep track of the unique edges and use those along with the support point's index to make new faces.

			std::vector<std::pair<size_t, size_t>> uniqueEdges;

			for (size_t i = 0; i < normals.size(); i++) {
				if (SameDirection(normals[i], support)) {
					size_t f = i * 3;

					AddIfUniqueEdge(uniqueEdges, faces, f,     f + 1);
					AddIfUniqueEdge(uniqueEdges, faces, f + 1, f + 2);
					AddIfUniqueEdge(uniqueEdges, faces, f + 2, f    );

					faces[f + 2] = faces.back(); faces.pop_back();
					faces[f + 1] = faces.back(); faces.pop_back();
					faces[f    ] = faces.back(); faces.pop_back();

					normals[i] = normals.back(); // pop-erase
					normals.pop_back();

					i--;
				}
			}

Now that we have a list of unique edges, we can add the new faces to a list and add the supporting point to the polytope. Storing the new faces in their own list allows us to calculate only the normals of these new faces.

			std::vector<size_t> newFaces;
			for (auto [edgeIndex1, edgeIndex2] : uniqueEdges) {
				newFaces.push_back(edgeIndex1);
				newFaces.push_back(edgeIndex2);
				newFaces.push_back(polytope.size());
			}
			 
			polytope.push_back(support);

			auto [newNormals, newMinFace] = GetFaceNormals(polytope, newFaces);

After we calculate the new normals, we need to find the new closest face. To eke out a little bit more performance, we'll only iterate over the old normals, then we'll compare the closest one to the closest face of the new normals. Finally, we can add these new faces and normals to the end of their respective lists.

			float oldMinDistance = FLT_MAX;
			for (size_t i = 0; i < normals.size(); i++) {
				if (normals[i].w < oldMinDistance) {
					oldMinDistance = normals[i].w;
					minFace = i;
				}
			}
 
			if (newNormals[newMinFace].w < oldMinDistance) {
				minFace = newMinFace + normals.size();
			}
 
			faces  .insert(faces  .end(), newFaces  .begin(), newFaces  .end());
			normals.insert(normals.end(), newNormals.begin(), newNormals.end());
		}
	}

Once a supporting point isn't found further from the closest face, we'll return that face's normal and its distance in a CollisionPoints.

	CollisionPoints points;
 
	points.Normal = minNormal;
	points.PenetrationDepth = minDistance + 0.001f;
	points.HasCollision = true;
 
	return points;
}

That's it for the main piece of the algorithm.

Don't think I forgot about the helper functions...

GetFaceNormals is just a slightly more complex version of the loop from the 2D version. Instead of i and j, we now get three vertices by first looking up their index in the faces list. In 3D the normal is found by taking the cross product of the vectors between the face's vertices. The winding order is now controlled by the index, instead of where we put some negative sign. Even though it's well defined, we don't check when adding new faces, so we still need the check here. Determining the winding involves finding the normal so there is no reason to not have this check here.

I've chosen to pack the distance and normal into a single vec4 to keep the code shorter.

std::pair<std::vector<vec4>, size_t> GetFaceNormals(
	const std::vector<vec3>& polytope,
	const std::vector<size_t>& faces)
{
	std::vector<vec4> normals;
	size_t minTriangle = 0;
	float  minDistance = FLT_MAX;

	for (size_t i = 0; i < faces.size(); i += 3) {
		vec3 a = polytope[faces[i    ]];
		vec3 b = polytope[faces[i + 1]];
		vec3 c = polytope[faces[i + 2]];

		vec3 normal = normalized(cross(b - a, c - a));
		float distance = dot(normal, a);

		if (distance < 0) {
			normal   *= -1;
			distance *= -1;
		}

		normals.emplace_back(normal, distance);

		if (distance < minDistance) {
			minTriangle = i / 3;
			minDistance = distance;
		}
	}

	return { normals, minTriangle };
}

AddIfUniqueEdge tests if the reverse of an edge already exists in the list and if so, removes it. If you look at how the winding works out, if a neighboring face shares an edge, it will be in reverse order. Remember, we only want to store the edges that we are going to save because every edge gets removed first, then we repair.

void AddIfUniqueEdge(
	std::vector<std::pair<size_t, size_t>>& edges,
	const std::vector<size_t>& faces,
	size_t a,
	size_t b)
{
	auto reverse = std::find(                       //      0--<--3
		edges.begin(),                              //     / \ B /   A: 2-0
		edges.end(),                                //    / A \ /    B: 0-2
		std::make_pair(faces[b], faces[a]) //   1-->--2
	);
 
	if (reverse != edges.end()) {
		edges.erase(reverse);
	}
 
	else {
		edges.emplace_back(faces[a], faces[b]);
	}
}

That's it for this algorithm, this allows us to handle some interesting collisions in the physics engine.

Let's look at some demos!