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, the 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 (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 at https://winter.dev/lilapps/gjk if you want to mess around with it for yourself.

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(polytopeshapeAshapeB) {
	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(shapeAshapeBminNormal);
		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 (C++)

We’ll start with a function that looks very similar to the original GJK function, but this time it takes in the Simplex as well as the Colliders.

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<vector3> polytope(simplex.begin(), simplex.end());
	std::vector<size_t>  faces = {
		0, 1, 2,
		0, 3, 1,
		0, 2, 3,
		1, 3, 2
	};

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

We’ll add the same main loop from before.

	vector3 minNormal;
	float   minDistance = FLT_MAX;
	
	while (minDistance == FLT_MAX) {
		minNormal   = normals[minFace].xyz();
		minDistance = normals[minFace].w;
 
		vector3 support = Support(colliderA, colliderB, minNormal);
		float sDistance = minNormal.dot(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(); 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 vector4 to keep the code shorter. 

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

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

		vector3 normal = (b - a).cross(c - a).normalized();
		float distance = normal.dot(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!

Comments

6 replies

Anonymous

can not access to you git code.

Iain

Which file are you looking for?

Foxar

I’ve attempted to implement the algorithm by following the article, but I’ve run into a bit of a issue, in which uniqueEdges vector will be empty as the AddIfUnique function will add then remove a lot of them.

I’ve added a if check and a break statement which just invalidates the entire EPA check returning empty penetration vector, but it happens very frequently, to the point where if i have a box above another, and let it fall, it will go all the way through the other one due to it always yielding empty unique edges list.

Any advice? Thank you!

Iain

hmm someone else was having trouble too. I’ve checked the working code that I have, and I’ve also added a check to see if it can’t find any edges to add. That seems to fix the issue but feels like a hack to me. After I finish with my exams, I’ll have to look and see if it’s caused by a logic error somewhere. My guess without digging into it is that the simplex is ordered wrong or something, maybe the faces are backwards?? I’ll have to take a look, thanks for pointing this out

(edit)
I put a breakpoint on the break statement I have in my code and I almost never hit it. I would look at the order of the faces first, I think that somewhere your order has gone wrong. This was one of the most common issues that I came accross. If you share your code I can take a quick look.

Anonymous

unfortunately that code doesnt work

Iain

Which part are you having trouble with?

Leave a Reply

Your email address will not be published.

Other Articles

Making an infinite world with Falling Sand part 2

Welcome back to the sand series, this post directly follows from what we did last time, so if you missed that, here’s the link. In this post we’ll first split the world into chunks, then look at some ways to speed it up. Splitting the world into chunks The main feature that chunks allow for […]

March 27, 2021
Making games with Falling Sand part 1

I want to get familiar with the process of releasing a game before I finish Metal Sphere Rising, so I’m planning on making a game in a month, and then releasing it on Steam or something. My brother and I were talking about it and came up with the idea of a space version of […]

December 30, 2020
Mesh generation

This post is less about some specific information and more about something I’ve been cooking up over at winter.dev/prims. When working in 3D everything is made out of meshes. In their simplest form these are big lists of positions that get fed into the GPU for rendering. These lists can be generated or loaded from […]

October 6, 2020
GJK: Collision detection algorithm in 2D/3D

In my last article, I only covered sphere vs. sphere collisions because they are the simplest to compute. Spheres are nice and all, but there comes a time when more complex shapes are needed. One popular algorithm for testing collisions is the Gilbert–Johnson–Keerthi algorithm, or GJK for short. With it we can detect collisions between […]

August 29, 2020
Designing a physics engine

By coincidence, right when The Cherno announced his game engine series I was just starting to get going on my own engine. I couldn’t wait to finally have a professional opinion on how to make one. With self-taught programming it’s hard to not doubt yourself constantly, wondering if you are doing things right or just […]

July 31, 2020
Another way of programming, taking it slow

Whenever I sit down to write some code, I always get an itch to finish whatever problem is staring me in the face right at that moment. Over the last 2 years, I’ve realized that if you can afford to tackle a problem over a long period of time, you should absolutely go for that […]

July 6, 2020
World 1 demo brings you to the outer forests

Calling all playtesters, After 2 months from the last playtest, the third demo is ready for review. This one brings the first real graphics to the game; with the addition of Voxel Cone Tracing there’s a warm glow to the whole forest. Right now that’s the biggest time sink per-frame so I am doing some […]

July 4, 2020

Projects

Game engine

IwEngine is an engine that I started as a way to lean about how computer games are made. Right now I am trying to make my first publishable game with it. I started by making something that was linear and story based and got about 50% done with it, but I wanted to try and publish something smaller and more arcade like first. That has turned into these sand games...

Mesh generation

Every shape has some method of generating a mesh for it, but there is no good central spot. This website will eventially contain a full list of differnt algorithms for every shape.

YouTube Subscriber tracker

Youtube removed the exact subscriber count. This resolves that issue and graphs my count ever hour.

Support

BY MAIL

Support with your eyes. I enjoy writing these posts and editing the videos that go along with them. It is very satisfying reading the comments and seeing people enjoying and hopefully learning something from what I make. Sign up to get an email notification for whenever I make a new post. It seems like my pace is around once a month, so don't worry about spam :)


ON YOUTUBE

Did you know I make video version of these posts? Check out the 5-10 minute condenced versions over on YouTube. I end up putting about the same amount of time into them as the posts themselves, so would appreciate a view!

ON TWITTER

Over on Twitter I post updates about videos while they're being made along with other random thoughts. If that sounds more your speed, I'd appreciate a follow :)