The last installment talked about the GJK algorithm as it pertains to collision detection. The original algorithm actually is used to obtain the distance and closest points between two convex shapes.

1. Introduction
2. Overview
3. Minkowski Sum
4. The Distance
5. Iteration
6. Closest Points
7. Convex Hull

Introduction
The algorithm uses much of the same concepts to determine the distance between the shapes. The algorithm is iterative, uses the Minkowski Sum/Difference, looks for the origin, and uses the same support function.

Figure 1: Two Separated Shapes

Overview
We know that if the shapes are not colliding that the Minkowski Difference will not contain the origin. Therefore instead of iteratively trying to enclose the origin with the simplex, we will want to generate a simplex that is closest to the origin. The closest simplex will always be on the edge of the Minkowski Difference. The closest simplex for 2D could be either a single point or a line.

Figure 2: The Minkowski Difference

Minkowski Sum
Just as we did for the collision detection portion of GJK in the previous post, the algorithm also needs to know the Minkowski Sum (Difference is what I will call it, see the GJK post).

Taking the same shapes from the GJK post and separating them (Figure 1) yeilds the same Minkowski Difference only translated slightly (Figure 2). We notice that the origin is not contained within the Minkowski Difference, therefore there is not a collision.

The Distance
The distance can be calculated by finding the closest point on the Minkowski Difference to the origin. The distance is then the magnitude of the closest point. By inspection, we see that the edge created by the points (-4, -1) and (1, 3) is the closest feature to the origin. Naturally the closest point to the origin is the point on this edge that forms a right angle to the origin. We can calculate the point by:

A = (-4, -1)
B = (1, 3)
// create the line
AB = (1, 3) - (-4, -1) = (5, 4)
AO = (0, 0) - (-4, -1) = (4, 1)
// project AO onto AB
AO.dot(AB) = 4 * 5 + 1 * 4 = 24
// get the length squared
AB.dot(AB) = 5 * 5 + 4 * 4 = 41
// calculate the distance along AB
t = 24 / 41
// calculate the point
AB.mult(t).add(A) = (120 / 41, 96 / 41) + (-4, -1)
                  = (-44 / 41, 55 / 41)
                  ≈ (-1, 1.3)
d = (-1, 1.3).magnitude() ≈ 1.64

This is a simple calculation since we know what points of the Minkowski Difference to use.

Figure 3: The Minimum Distance

Iteration
Like the collision detection routine of GJK the distance routine is iterative (and almost identical for that matter). We need to iteratively build a simplex which contains the closest points on the Minkowski Difference to the origin. The points will be obtained using a similar method of choosing a direction, using the support function, and checking for the termination case.

Lets examine some psuedo code:

// exactly like the previous post, use whatever 
// initial direction you want, some are more optimal
d = // choose a direction
// obtain the first Minkowski Difference point using
// the direction and the support function
Simplex.add(support(A, B, d));
// like the previous post just negate the
// the prevous direction to get the next point
Simplex.add(support(A, B, -d));
// start the loop
while (true) {
  // obtain the point on the current simplex closest
  // to the origin (see above example)
  p = ClosestPointToOrigin(Simplex.a, Simplex.b);
  // check if p is the zero vector
  if (p.isZero()) {
    // then the origin is on the Minkowski Difference
    // I consider this touching/collision
    return false;
  }
  // p.to(origin) is the new direction
  // we normalize here because we need to check the
  // projections along this vector later
  d = p.negate().normalize();
  // obtain a new Minkowski Difference point along
  // the new direction
  c = support(A, B, d);
  // is the point we obtained far enough along d?
  double dc = c.dot(d);
  // you can use a or b here it doesn't matter
  double da = Simplex.a.dot(d);
  // tolerance is how acurate you want to be
  if (dc - da < tolerance) {
    // if we got closer, given some tolerance,
    // to the origin, then we can assume that we
    // are done
    distance = dc;
    return true;
  }
  // if we are still getting closer then only keep
  // the points in the simplex that are closest to
  // the origin (we already know that c is closer
  // than both a and b)
  // the magnitude is the same as distance(origin, a)
  if (a.magnitude() < b.magnitude()) {
    b = c;
  } else {
    a = c;
  }
}

The first few lines look a lot like the previous GJK post. The difference is the building of our simplex. We are using the same idea of a simplex, we use the same support function and roughly the same logic, however, we only keep 2 points at all times (3D would be 3 points) and we find a point on the simplex closest to the origin instead of finding the Voronoi region that the origin lies in.

As we did in the previous post this is best explained by running through an example. Let’s take the example above from Figure 1 and run through the iterations.

Pre Iteration:

// im going to choose the vector joining the
// centers of the objects as the initial d
d = (11.5, 4.0) - (5.5, 8.5) = (6, -4.5)
Simplex.add(support(A, B, d)) = (9, 9) - (8, 6) = (1, 3)
Simplex.add(support(A, B, -d)) = (4, 11) - (13, 1) = (-9, 10)
// start the iterations

Iteration 1:

// find the new d
// by inspection we can tell that (1, 3) is the closest
// the computation will yield the same result
p = (1, 3)
d = (-1, -3) ≈ (-0.32, -0.95)
// get a new point in this direction
c = support(A, B, d) = (4, 5) - (15, 6) = (-11, -1)
// is c far enough along d
dc = 3.52 + 0.95 = 4.47
da = -0.32 - 2.85 = -3.17
// 4.47 - -3.17 = 7.64 not small enough
maga = a.magnitude() = 3.16
magb = b.magnitude() = 13.45
// a is smaller so replace b with c
b = c

Figure 4: Pre Iteration Simplex

Figure 5: Post Iteration 1 Simplex

In this first iteration I didn’t go through the trouble of calculating p because its obviously going to be the end point a. If you performed the real calculation, like the code will do, you will obtain the same result. We notice that even though the closest point is a point already on the simplex, we can still use it as the next direction. We obtain a new point using the new direction and keep the two closest points.

Iteration 2:

// find the new d
// by inspection we can tell that we need to perform
// the closest point computation
a = (1, 3)
b = (-11, -1)
AB = (-11, -1) - (1, 3) = (-12, -4)
AO = (0, 0) - (1, 3) = (-1, -3)
AO.dot(AB) = -1 * -12 + -3 * -4 = 24
AB.dot(AB) = -12 * -12 + -4 * -4 = 160
t = 24 / 160 = 3 / 20
p = AB.mult(t).add(A) = (-36/20, -12/20) + (1, 3) = (-4/5, 12/5)
d ≈ (0.8, -2.4) ≈ (0.32, -0.95)
// get a new point in this direction
c = support(A, B, d) = (4, 5) - (8, 6) = (-4, -1)
// is c far enough along d
dc = -1.28 + 0.95 = -0.33
da = 0.32 - 2.85 = -2.53
// -0.33 - -2.53 = 2.2 not small enough
maga = a.magnitude() = 3.16
magb = b.magnitude() = 11.05
// a is smaller so replace b with c
b = c

Figure 6: Post Iteration 2 Simplex

Notice that our simplex is getting closer to the origin.

Iteration 3:

// find the new d
// by inspection we can tell that we need to perform
// the closest point computation
a = (1, 3)
b = (-4, -1)
AB = (-4, -1) - (1, 3) = (-5, -4)
AO = (0, 0) - (1, 3) = (-1, -3)
AO.dot(AB) = -1 * -5 + -3 * -4 = 17
AB.dot(AB) = -5 * -5 + -4 * -4 = 41
t = 17 / 41
p = AB.mult(t).add(A) = (-85/41, -68/20) + (1, 3) = (-44/41, 55/41)
d ≈ (-1.07, 1.34) ≈ (0.62, -0.78)
// get a new point in this direction
c = support(A, B, d) = (9, 9) - (8, 6) = (1, 3)
// is c far enough along d
dc = 0.62 - 2.34 = -1.72
da = 0.62 - 2.34 = -1.72
// -1.72 - -1.72 = 0 i think thats small enough!
// we done!
distance = 1.72

We notice that when we terminated the difference in projections was zero. This can only happen with two polygons. If either A or B was a shape with a curved edge the difference in projections would approach zero but not obtain zero. Therefore we use a tolerance to handle curved shapes since the tolerance will also work for polygons.

Another problem curved shapes introduce is that if the chosen tolerance is not large enough the algorithm will never terminate. In this case we add a maximum iterations termination case.

One more problem that this algorithm can run into is if the shapes are actually intersecting. If they are intersecting the algorithm will never terminiate. This isn’t as big of a problem since most of the time you will determine if the shapes are colliding first anyway. If not, then we must add a check in the while loop for the simplex containing the origin. This can be done by a simple point in triangle test (2D).

Closest Points
In addition to the distance between the two shapes, we can also determine the closest points on the shapes. To do this we need to store additional information as we progress through the algorithm. If we store the points on the shapes that were used to create the Minkowski Difference points we can use them later to determine the closest points.

For instance, we terminated above with the Minkowski Difference points A = (1, 3) and B = (-4, -1). These points were created by the following points on their respective shapes:

The points used to create the Minkowski Difference points are not necessarily the closest points. However, using these source points we can calculate the closest points.

Convex Hull
We see that the points from A and B are not the correct closests points. By inspection we can see that the closest point on B is (8, 6) and the closest point on A is roughly (6.75, 7.25). We must do some calculation to obtain the closest points. Here’s where the definition of a Convex Hull comes in:

CH(S) = ∑_i=1…n λ_iP_i = λ₁P₁ + … + λ_nP_n
where P_i∈S, λ_i∈R
and ∑_i…n λ_i = 1
where λ_i >= 0

What this definition implies is that any point on the convex hull of the set of points S can be defined by all the points that make up the convex hull multiplied by some lambda value.

Our 2D example then would look like this:

CH(S) = λ₁P₁ + λ₂P₂

Now, if we say that Q is the closest point to the origin on the termination simplex, then we know that the vector from Q to the origin must be perpendicular to the segment that Q lies on.

L = A – B
Q · L = 0

If we substitute for Q we obtain:

(λ₁A + λ₂B) · L = 0

We need to solve for λ₁ and λ₂ but to do so we need 2 equations. The second equation comes from the other part of the definition of a convex hull:

λ₁ + λ₂ = 1

Solving the system of equations we obtain:

λ₂ = -L · A / (L · L)
λ₁ = 1 – λ₂

If we perform this computation on our example above, we get:

L = (-4, -1) - (1, 3) = (-5, -4)
LdotL = 25 + 16 = 41
LdotA = -5 - 12 = -17
λ2 = 17/41
λ1 = 24/41

After computing λ₁ and λ₂ we can compute the closest points by using the definition of the convex hull again, but using the points that made up the Minkowski Difference points:

Aclosest = λ1As1 + λ2Bs1
Bclosest = λ1As2 + λ2Bs2

Aclosest = 24/41 * (9, 9) + 17/41 * (4, 5) ≈ (6.93, 7.34)
Bclosest = 24/41 * (8, 6) + 17/41 * (8, 6) = (8, 6)

As we can see we computed the closest points!

There are a couple of problems here we must resolve however.

First, if the Minkowski Difference points A and B are the same point, then L will be the zero vector. This means that later when we divide by the magnitude of L we will divide by zero: not good. What this means is that the closest point to the origin is not on a edge of the Minkowski Difference but is a point. Because of this, the support points that made both A and B are the same, and therefore you can just return the support points of A or B:

if (L.isZero()) {
  Aclosest = A.s1;
  Bclosest = A.s2;
}

The second problem comes in when either λ₁ or λ₂ is negative. According to the definition of a convex hull, λ must be greater than zero. If λ is negative, this tells us that the support points of the other Minkowski Difference point are the closest points:

if (lambda1 < 0) {
  Aclosest = B.s1;
  Bclosest = B.s2;
} else if (lambda2 < 0) {
  Aclosest = A.s1;
  Bclosest = A.s2;
}