Finding the point on a simplex closest to the origin using GJK's distance subalgorithm

Question

I'm trying to implement the Gilbert–Johnson–Keerthi distance algorithm (GJK), but I'm having problems with the "distance subalgorithm", also known as "Johnson's Algorithm", which is used to determine the point on a simplex that is closest to the origin. I'm getting incorrect results but I can't find any bugs in my code, so the problem must be in my interpretation of the algorithm.

In Johnson’s Algorithm (as described in Gino van den Bergen's book Collision Detection in Interactive 3D Environments), the point on the affine hull of a simplex X = {yi : i ∈ Ix} closest to the origin is given by:

Where the Δi^X values are determined recursively in order of increasing cardinality of X:

... and Δ^X is given by:

For two dimensions, I find the closest point to the origin using:

Point ClosestPointToOrigin(Simplex simplex)
{
    float dx = 0;
    for (int i = 0; i < simplex.size(); ++i)
        dx += dj(simplex, i);

    Point closest_point(0,0);
    for (int i = 0; i < simplex.size(); ++i)
        closest_point += dj(simplex, i) / dx * simplex[i];

    return closest_point;
}

In which the Δi values are determined by:

float dj(Simplex simplex, int j)
{
    if (j == 0)
    {
        return 1;
    }
    else
    {
        float d = 0;

        for (int i = 0; i < j; ++i)
            d += dj(simplex, i) * (simplex[0] - simplex[j]).dotProduct(simplex[i]);

        return d;
    }
}

For a simplex X = {y1, y2} where y1 = (1,1), y2 = (1,-1), the above code returns (1.0, -0.333333), when the closest point is, in fact, (1, 0).

I must be doing something wrong, but I can't figure out what that is.

Answer 1

Your error is the dj function, maybe you have misunderstood the dxi equation or you did not write what you want.

I will try to explain myself, do not hesitate to comment if you do not understand something (I am writing pseudo python code but it should be easily understandable).

Assume I have the following Simplex:

S  = Simplex ({
    1: Point (1, 1) # y1
    2: Point (1,-1) # y2
})

I can immediately compute 2 deltas values:

Then, I can compute 2 others deltas values:

Hopefully by now you'll start to see your mistake: The Δ values are index based, so for each Simplex X of dimension n, you have n Δ values. One of your mistake was to assume that you can compute Δ^X₀ and Δ^X_i regardless of the content of X, which is false.

Now the last Δ:

Notice that:

Once you are here:

Here is a code written in Python, if you do not understand it, I'll try to write one in a language you understand:

import numpy

class Point (numpy.ndarray):
    def __new__ (cls, x, y):
        return numpy.asarray([x, y]).astype(float).view(cls)

    def __str__ (self):
        return repr(self)

    def __repr__ (self):
        return 'Point ({}, {})'.format(self.x, self.y)

    x = property (fget = lambda s: s[0])
    y = property (fget = lambda s: s[1])

class Simplex (dict):
    def __init__ (self, points):
        super(Simplex, self).__init__ (enumerate(points))

    def __str__ (self):
        return repr(self)

    def __repr__ (self):
        return 'Simplex <' + dict.__repr__ (self) + '>'

def closest_point (s):
    dx = sum(dj(s, i) for i in range(len(s)))
    return sum(dj(s, i) / dx * v for i, v in s.items())

def dj (s, j):
    if len(s) == 0 or (len(s) == 1 and not j in s):
        print(s, j)
        raise ValueError ()
    if len(s) == 1:
        return 1
    ts = s.copy()
    yj = s[j]
    del ts[j]
    return sum(dj(ts, i) * (ts[list(ts.keys())[0]] - yj).dot(v) for i, v in ts.items())

S = Simplex([Point(1, 1), Point(1, -1)])

print(closest_point (S))