Convex polyhedra collision test (GJK Algorithm)

10 minute read

Published:

If we interpret a polyhedron as a set of points, two polyhedra represented by sets \(A\) and \(B\), for example, collide if \(A \cap B \neq \emptyset\). The intersection set \(A \cap B\) represents all pairs of points between \(A\) and \(B\) which have distance \(0\) between them, because well… each of these pairs is composed by the same point (shared by \(A\) and \(B\)).

A very nice operation between sets of points is the Minkowski Sum:

Let \(A\) and \(B\) be two point sets. The Minkowski sum \(A \oplus B\) is defined as the set \(A \oplus B = \{ a + b : a \in A, b \in B\}.\) The Minkowski difference is obtained by \(A \ominus B = A \oplus (-B) \).


The Minkowski sum is very useful because it can give us the distance between two sets of points \(A\) and \(B\):

\[distance(A, B) = min \{ \parallel c\parallel : c \in A \ominus B \}.\]

The Euclidian distance between two polyhedra is equivalent to the distance between their Minkowski difference and the origin.

For two convex polyhedra, \(A\) and \(B\), the Minkowski Sum \(C = A \oplus B\) has the following properties:

  • \(C\) is a convex polyhedron;
  • The vertices of \(C\) are sums of vertices of \(A\) and \(B\).

Thus, the collision exists if and only if \(C\) contains the origin. The red region bellow represents the Minkowski difference set of the two shapes, play around with the vertices to visualize the final set, notice the origin point.

Gilbert - Johnson - Keerthi Algorithm

In short, the GJK algorithm tests if two objects \(A\) and \(B\) are colliding by checking if \(0 \in A \ominus B\) is true (simply \(distance(A, B)\) ). Although it seems very straightforward (and it is indeed), the real magic and beauty of the GJK algorithm is how \(distance\) is implemented (A very good description of the algorithm is given by Casey Muratori), but first, a simple observation:

The resulting Minkowski Sum of two convex polyhedra is also a convex polyhedron. Since all we care about is to check if \(0\) belongs to the final polyhedron, we only need to focus on the vertices of these geometric shapes (the convex hull), because any operation with interior points will lead to interior points of the resulting shape.

The brute force algorithm would be to compute all pairs of vertices between the polyhedra, which leaves us with quadratic complexity. In the GJK algorithm on the other hand, instead of computing the entire set \(C = A \ominus B\) explicitly, it only computes points necessary to find the point in \(C\) that is closest to the origin. The GJK algorithm samples these points using a support mapping of \(C\).


Briefly, a support mapping is a function that maps a given direction (d) into a supporting point for the convex polyhedron (A). \(S_A(d) = max \{ d^Tp, p \in A \}.\)


In our 2D code, we could have something like this:

struct ConvexPolygon {
    vec2 support(vec2 d) {
      // for all vertices, find for which index the value 
      // of dot(vertices[i], d) is greater
      ...
      // instead of a O(n) algorithm here, we can use hill climbing 
      // search. Assuming our vertex list is topologically sorted
      return vertices[max_dot_i];
    }
    // suppose vertices form a convex shape
    std::vector< vec2 > vertices;
};

Since \(A \ominus B\) is a linear operation, we have

\[S_{A \ominus B}(d) = max \{d^Tp, p \in A \ominus B\}\] \[= max \{ d^Ta - d^Tb, a \in A, b \in B \}\] \[= max \{ d^Ta, a \in A \} - max \{ -d^Tb, b \in B \}\] \[= S_A(d) - S_B(-d)\]

It means that the support function of the Minkowski difference can be computed from the supporting points of the individual polyhedra \(A\) and \(B\). Remember, the support function will help the algorithm to “walk” towards the origin, which is our point of interest, so we will choose the direction \(d\) based on this goal. But how do we know the right direction to pick? This interesting theorem will help us answering this question:

Carathéodory’s theorem

For a convex body \(H\) of \( \mathbb{R}^d \) each point of \(H\) can be expressed as the convex combination of no more than \(d + 1\) points from \(H\).


Ok, it didn’t answered directly our question, but soon it is going to make sense. The message behind the theorem is that each point of a polyhedron needs no more than 4 points of that polyhedron to be expressed, in fact this sub-set of \(d + 1\) points has a name:


Suppose \(d + 1\) points \(p_0, \dots, p_d \in \mathbb{R}^d \) are affinely independent, than the set of points \(S = \{ \theta_0 p_0 + \dots + \theta_d p_d \mid \theta_i \geq 0, 0 \leq i \leq d, \sum_{0}^{d} \theta_i = 1\}\) is named k-Simplex. In other words, the simplex is the simplest polygon of its dimension, here are the 0-Simplex, 1-Simplex, 2-Simplex and 3-Simplex in order:


Since our Minkowski difference is a convex body it means that we can split it into a set of simplices and search for the origin inside them.

However, we don’t need do this explicitly, otherwise we would need to compute the Minkowski difference set explicitly too. The strategy is to iteratively create a new simplex each step that contains points closer to the origin than the step before until the origin happens to be inside the current simplex or it be proved to be outside. We start with a 0-Simplex and keep updating with new points (creating a 1-Simplex, then a 2-Simplex and so on (up to \(d\)-Simplex)) until the process is finished.

Before my text get even more confuse lets take a look at some code (2-dimensional case) to have a more general idea of the whole process.

bool testCollision(const ConvexPolygon& a, const ConvexPolygon& b) {
  // start with an arbitrarily direction for the support mapping
  vec2 d(1, 0);
  // simplex vertices (since it is 2D, we have at most 3 vertices)
  vec2 s[3];
  // number of vertices of the current simplex, initially an empty simplex
  int k = 0;
  while(1) {
    // sample a new point from the Minkowski difference set
    vec2 p = a.support(d) - b.support(-d);
    // check if the origin outside the set
    if(dot(p, d) < 0)
      return false;
    // build and test the new simplex and compute the new direction d
    if(buildAndTestSimplex(s, p, k, d))
      return true;
  }
}

As you may notice, the code is quite simple (and it really is!), each step we “jump” in the direction of the origin to a new simplex of our set of simplicies that exists implicitly and look for the origin point (not exactly, since there is more than one possible set of simplicies we are jumping between different simplicies of different sets… but that is not important here). The real magic though, is inside buildAndTestSimplex, each step we need to decide what direction to take using the current simplex, here is what happens:

first step (\(k = 1\))

First we start with a single point \(p\) of our \(A \ominus B\) set. There is not much to do here. The new direction to take is \(-p\).

second step (\(k = 2\))

Now we have a 1-Simplex. As you can see, our plane is divided into 4 regions where the origin can be found. The first observation is that regions 1 and 4 don’t contain the origin because the vertices were found by the support function in each direction of each of these regions, it means that there are no more points of the \(A \ominus B\) set there. If the origin was in regions 1 or 4 then the algorithm would had stopped at line 12.

So the origin is certainly in 2 or 3 and the new direction is

\(v = \overline{S_0S_1}\) \(\begin{cases} & d = (-v_y, v_x), & (v \times -S_0)_z > 0\\\ & d = (v_y, -v_x), & (v \times -S_0)_z < 0 \end{cases}\)

\(n^{th}\) step (\(k = n, n > 2\))

As the same case above, we observe our plane divided:

542px-four-level_z-svg

If we keep the order of our vertices and use the same logic to exclude the regions 1 and 4 of the second step, then we can exclude regions 1, 2, 3 and 7 of this step. To save some computations, we can verify first if the origin is in region 5 and 6, and if is not then certainly is in region 4.
Defining \(v_{02} = \overline{S_0S_2}, v_{12} = \overline{S_1S_2}\)

If \((v_{02} \times -S_2).z > 0 \) then the origin is in 5 and the new direction is \((-v_{02}.y, v_{02}.x)\).

If \((v_{12} \times -S_2).z < 0\) then the origin is in 6 and the new direction is \((v_{12}.y, -v_{12}.x)\).

Otherwise we return true.
Note: remember to arrange the order of the vertices conveniently so these equations work properly.

Here is a very simple example of the algorithm in action:

The code:

// the z coordinate of the cross product of vectors (a.x, a.y, 0) 
// and (b.x, b.y, 0)
float cross2D(const vec2& a, const vec2& b) {
  return a.x * b.y - a.y * b.x;
}
bool buildAndTestSimplex(vec2 s[], vec2 p, int &k, vec2 &d) {
  k = std::min(k + 1, 3);
  s[k - 1] = p;
  if(k == 1) {
    D = -s[0];
    return false;
  }
  if(k == 2) {
    vec2 a = s[1] - s[0];
    if(cross(a,-s[0]) < 0) {
      s[2] = s[1];
      s[1] = s[0];
      s[0] = s[2];
      D = a.right();
    }
    else D = a.left();
    return false;
  }
  vec2 a = s[2] - s[0];
  if(cross(a, -s[0]) > 0) {
    D = a.left();
    s[1] = s[2];
    k--;
    return false;
  }
  vec2 b = s[2] - s[1];
  if(cross(b, -s[1]) < 0) {
    D = b.right();
    s[0] = s[2];
    k--;
    return false;
  }
  return true;
}

The 3D case follows the same idea, the difference is that in case \(k = 3\), we have to check which side of the triangle plane the origin is and construct a tetrahedron. The cross products for the other cases must be adapted as well.