Johnson's distance algorithm: can't find smallest simplex
Posted: Mon May 01, 2017 11:30 pm
I'm having a problem with Johnson's distance algorithm where it sometimes fails to find the smallest simplex containing the point that is closest to the origin. In 2D, this means I get a simplex of size 3 that does not surround the origin and can't be reduced any further by the algorithm (as implemented). Ideally this would never happen, but in my tests it happens a lot.
Here's an example of such a simplex (in 2D). Notice how two of the vertices lie close together and the origin is not included:
I'd like to know if such behavior is due to precision issues or if there's something wrong with my implementation of the algorithm.
Here's the code that finds the smallest simplex containing the point closest to the origin:
If there's nothing wrong with the code, then perhaps there's a way to special case these kinds of simplices to find the true closest point?
For completeness, this is my GJK code:
Here's an example of such a simplex (in 2D). Notice how two of the vertices lie close together and the origin is not included:
Code: Select all
[(123.881470, 2.262878) (-132.185287, 2.262817) (123.814880, 2.262817)]
Here's the code that finds the smallest simplex containing the point closest to the origin:
Code: Select all
/* Johnson's distance algorithm.
parameters:
closest_point (out) - The point on the simplex closest to the origin.
witness_a (out) - Witness point for the collision on the first collider.
witness_b (out) - Witness point for the collision on the second collider.
ray_sample (in) - Used as origin for the simplex, for use with continuous GJK; use null for regular GJK.
return value:
smallest subset X of vertices Y in the original simplex such that closest_point is in convex hull of X
notes:
points[n].x - The vertices (x,y) of the simplex as obtained via Minkowski difference.
points[n].a - Corresponding support point for the first collider in world space.
points[n].b - Corresponding support point for the second collider in world space.
det_y0_y1_0 - determinant i=0 for simplex {y0, y1} = {y1} union {y0}.
det_y0_y1_1 - determinant i=1 for simplex {y0, y1} = {y0} union {y1}.
det_y0_y2_0 - determinant i=0 for simplex {y0, y2} = {y2} union {y0}.
det_y0_y2_2 - determinant i=2 for simplex {y0, y2} = {y0} union {y2}.
det_y1_y2_1 - determinant i=1 for simplex {y1, y2} = {y2} union {y1}.
det_y1_y2_2 - determinant i=2 for simplex {y1, y2} = {y1} union {y2}.
det_y0_y1_y2_0 - determinant i=0 for simplex {y0, y1, y2} = {y1, y2} union {y0}.
det_y0_y1_y2_1 - determinant i=1 for simplex {y0, y1, y2} = {y0, y2} union {y1} .
det_y0_y1_y2_2 - determinant i=2 for simplex {y0, y1, y2} = {y0, y1} union {y2}.
*/
Simplex Simplex::reducedSet(Vector2D &closest_point, Vector2D &witness_a, Vector2D &witness_b, const Vector2D *ray_sample) const
{
Vector2D tpoints[3];
if (ray_sample)
{
/* Translate simplex vertices to make *ray_sample the origin. Used for continuous GJK. */
tpoints[0] = *ray_sample - points[0].x;
tpoints[1] = *ray_sample - points[1].x;
tpoints[2] = *ray_sample - points[2].x;
}
else
{
/* Simplex origin at (0,0). Used for discrete GJK. */
tpoints[0] = points[0].x;
tpoints[1] = points[1].x;
tpoints[2] = points[2].x;
}
if (this->size == 1)
{
witness_a = points[0].a;
witness_b = points[0].b;
closest_point = tpoints[0];
return Simplex(points[0]);
}
else if (this->size == 2)
{
const float det_y0_y1_0 = (tpoints[1] - tpoints[0]).dotProduct(tpoints[1]);
const float det_y0_y1_1 = (tpoints[0] - tpoints[1]).dotProduct(tpoints[0]);
if (det_y0_y1_1 <= 0)
{
witness_a = points[0].a;
witness_b = points[0].b;
closest_point = tpoints[0];
return Simplex(points[0]);
}
else if (det_y0_y1_0 <= 0)
{
witness_a = points[1].a;
witness_b = points[1].b;
closest_point = tpoints[1];
return Simplex(points[1]);
}
else
{
const float dx = det_y0_y1_0 + det_y0_y1_1;
const float l0 = det_y0_y1_0 / dx;
const float l1 = det_y0_y1_1 / dx;
witness_a = points[0].a.mul(l0) + points[1].a.mul(l1);
witness_b = points[0].b.mul(l0) + points[1].b.mul(l1);
closest_point = tpoints[0].mul(l0) + tpoints[1].mul(l1);
return Simplex(points[0], points[1]);
}
}
else
{
const float det_y0_y1_1 = (tpoints[0] - tpoints[1]).dotProduct(tpoints[0]);
const float det_y0_y2_2 = (tpoints[0] - tpoints[2]).dotProduct(tpoints[0]);
const float det_y0_y1_0 = (tpoints[1] - tpoints[0]).dotProduct(tpoints[1]);
const float det_y1_y2_2 = (tpoints[1] - tpoints[2]).dotProduct(tpoints[1]);
const float det_y0_y2_0 = (tpoints[2] - tpoints[0]).dotProduct(tpoints[2]);
const float det_y1_y2_1 = (tpoints[2] - tpoints[1]).dotProduct(tpoints[2]);
const float det_y0_y1_y2_0 = det_y1_y2_1 * (tpoints[1] - tpoints[0]).dotProduct(tpoints[1]) + det_y1_y2_2 * (tpoints[1] - tpoints[0]).dotProduct(tpoints[2]);
const float det_y0_y1_y2_1 = det_y0_y2_0 * (tpoints[0] - tpoints[1]).dotProduct(tpoints[0]) + det_y0_y2_2 * (tpoints[0] - tpoints[1]).dotProduct(tpoints[2]);
const float det_y0_y1_y2_2 = det_y0_y1_0 * (tpoints[0] - tpoints[2]).dotProduct(tpoints[0]) + det_y0_y1_1 * (tpoints[0] - tpoints[2]).dotProduct(tpoints[1]);
if (det_y0_y2_0 <= 0 && det_y1_y2_1 <= 0)
{
witness_a = points[2].a;
witness_b = points[2].b;
closest_point = tpoints[2];
return Simplex(points[2]);
}
else if (det_y0_y1_0 <= 0 && det_y1_y2_2 <= 0)
{
witness_a = points[1].a;
witness_b = points[1].b;
closest_point = tpoints[1];
return Simplex(points[1]);
}
else if (det_y0_y1_1 <= 0 && det_y0_y2_2 <= 0)
{
witness_a = points[0].a;
witness_b = points[0].b;
closest_point = tpoints[0];
return Simplex(points[0]);
}
else if (det_y0_y1_y2_0 <= 0 && det_y1_y2_1 > 0 && det_y1_y2_2 > 0)
{
const float dx = det_y1_y2_1 + det_y1_y2_2;
const float l1 = det_y1_y2_1 / dx;
const float l2 = det_y1_y2_2 / dx;
witness_a = points[1].a.mul(l1) + points[2].a.mul(l2);
witness_b = points[1].b.mul(l1) + points[2].b.mul(l2);
closest_point = tpoints[1].mul(l1) + tpoints[2].mul(l2);
return Simplex(points[1], points[2]);
}
else if (det_y0_y1_y2_1 <= 0 && det_y0_y2_0 > 0 && det_y0_y2_2 > 0)
{
const float dx = det_y0_y2_0 + det_y0_y2_2;
const float l0 = det_y0_y2_0 / dx;
const float l2 = det_y0_y2_2 / dx;
witness_a = points[0].a.mul(l0) + points[2].a.mul(l2);
witness_b = points[0].b.mul(l0) + points[2].b.mul(l2);
closest_point = tpoints[0].mul(l0) + tpoints[2].mul(l2);
return Simplex(points[0], points[2]);
}
else if (det_y0_y1_y2_2 <= 0 && det_y0_y1_0 > 0 && det_y0_y1_1 > 0)
{
const float dx = det_y0_y1_0 + det_y0_y1_1;
const float l0 = det_y0_y1_0 / dx;
const float l1 = det_y0_y1_1 / dx;
witness_a = points[0].a.mul(l0) + points[1].a.mul(l1);
witness_b = points[0].b.mul(l0) + points[1].b.mul(l1);
closest_point = tpoints[0].mul(l0) + tpoints[1].mul(l1);
return Simplex(points[0], points[1]);
}
else if (det_y0_y1_y2_0 > 0 && det_y0_y1_y2_1 > 0 && det_y0_y1_y2_2 > 0)
{
witness_a = Vector2D(0,0);
witness_b = witness_a;
closest_point = Vector2D(0,0);
return Simplex(points[0], points[1], points[2]);
}
else
{
return Simplex();
}
}
}
For completeness, this is my GJK code:
Code: Select all
float GJK::Distance(Vector2D &closest_point_a, Vector2D &closest_point_b, CollisionShape &a, const Vector2D &a_position, CollisionShape &b, const Vector2D &b_position)
{
const float error_tolerance = std::numeric_limits<float>::epsilon() * 100.0;
const float relative_error = std::numeric_limits<float>::epsilon() * 100.0;
Simplex simplex;
Simplex expanded_simplex;
Vector2D v = a.arbitraryPoint(a_position) - b.arbitraryPoint(b_position);
do {
Vector2D sa = a.supportPoint(-v, a_position);
Vector2D sb = b.supportPoint(v, b_position);
Vector2D support = sa - sb;
if (expanded_simplex.includesVertex(support) || v.magnitudeSquared() - v.dotProduct(support) <= relative_error * v.magnitudeSquared())
return v.magnitude();
expanded_simplex = simplex.Union(SimplexVertex(support, sa, sb));
simplex = expanded_simplex.reducedSet(v, closest_point_a, closest_point_b, 0);
if (simplex.length() == 0)
return v.magnitude();
} while (simplex.length() != 3 && v.magnitudeSquared() > error_tolerance * simplex.maximumVectorMagnitudeSquared());
return 0;
}