Finding a point on a simplex closest to the origin using the GAL
I am trying to implement the Gilbert-Johnson-Keerthi (GJK) distance algorithm , but I am having problems with the "far subalgorithm", also known as the "Johnson-Algorithm", which is used to locate a point on a simplex closest to the origin. I am getting wrong results, but I cannot find errors in my code, so the problem must be in my interpretation of the algorithm.
In Johnson's algorithm (as described in the book "Collision Detection by Gino van den Bergen" in Interactive 3D Environments), the point on the affine hull of a simplex X = {yi : i β Ix}
closest to the origin is defined as:
Where the values ββof Ξi ^ X are determined recursively in ascending order of cardinality X:
... and Ξ ^ X is given by the formula:
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 values ββof Ξi are determined:
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 simplex X = {y1, y2}
, where y1 = (1,1)
, y2 = (1,-1)
above code returns (1.0, -0.333333)
when the closest point, in fact (1, 0)
.
I must be doing something wrong, but I can't figure out what it is.
source to share
Your mistake is a function dj
, you might have misunderstood the equation dxi
or didn't write what you want.
I'll try to explain myself, feel free to comment if you don't understand something (I'm writing python pseudo code, but that should be easy to understand).
Suppose I have the following simplex:
S = Simplex ({
1: Point (1, 1) # y1
2: Point (1,-1) # y2
})
I can calculate 2 delta values ββat once:
Then I can calculate 2 more deltas values:
Hopefully by now you will start to see your mistake: The & Delta; values ββare based on indices, so for every Simplex X of dimension n you have n & Delta; values. One of your mistakes was to assume that you can compute? Delta; X 0and? Delta; X i regardless of the content of X, which is false.
Now the last & Delta ;:
Note:
Once you are here:
Here is the code written in Python, if you do not understand it, I will try to write it 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))
source to share