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;
}