Real-Time Physics Simulation Forum

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:

Code: Select all

[(123.881470, 2.262878) (-132.185287, 2.262817) (123.814880, 2.262817)]

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:

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

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:

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

Posted: **Tue May 02, 2017 2:23 am**

I have added some comments to make the code a bit easier to understand. I should probably do a little refactoring.

Posted: **Sun May 07, 2017 7:56 pm**

Hi Adrian,

I haven't checked your code so I cannot tell whether there is a bug in there. I do know that due to rounding issues it can happen that no proper simplex is found. In fact, the original paper by Gilbert Johnson and Keerthi already mentions the issue and offers a solution in form of backup procedure. The backup procedure computes the closest point for each sub-simplex and simply keeps the closest one. This procedure is more expensive than the normal Johnson procedure but bullet proof (no pun). After a call to the backup procedure the GJK loop is terminated as the current simplex is probably too flat to give you proper results. Others ways to handle rounding noise are:

(a) Check whether a support point has already been returned earlier. This would work only for pairs of polytopes. (Super)quadrics have infinite support points and are likely to never return the same one.
(b) Check whether the new closet point is significantly closer to the origin than the one from previous iteration. If the difference in distance is less than some epsilon times the current distance then you have run into numerical problems. Return the point from previous iteration as the current one is probably too noisy.

Cheers,

Gino

Posted: **Mon May 08, 2017 3:37 am**

Hi Gino,

I'm already checking for duplicate support points as suggested in your book, to prevent GJK from looping indefinitely*. I'm not checking for the closest point being significantly closer to the origin than the last closest point, but Erwin suggested that too in a different thread so it's something I intend to try. At this point I'm thinking of doing as suggested by Christer in his book, checking the origin against the Voronoi regions of the simplex and choosing the closest feature that way. While mathematically equivalent to Johnson's algorithm, a number of people have said it can help alleviate numerical issues.

Thank you for your help.

* In my code I treat points that are very close together as being the same point. If I don't do that I get infinite loops when dealing with spheres and other shapes of its kind. I don't know if I'm supposed to do it that way, but it's the only way I could think of to fix the infinite loop issue while keeping to the algorithm as presented in your book.

Posted: **Wed May 10, 2017 8:23 am**

Adrian Lopez wrote:Hi Gino,

At this point I'm thinking of doing as suggested by Christer in his book, checking the origin against the Voronoi regions of the simplex and choosing the closest feature that way. While mathematically equivalent to Johnson's algorithm, a number of people have said it can help alleviate numerical issues.

Indeed the concept of Voronoi regions is already in the original Johnson algorithm. The sign tests on the lamba's are in fact sidedness checks against Voronoi planes. There are however different ways to compute the closest point on an affine hull. The original Johnson's algorithm solves for all barycentric coordinates of the closest points. The system matrix includes a row to constrain the point to lie on the affine hull (sum of barycentric coordinates is one). Of course, you could immediately eliminate one of the barycentric coordinates and solve a smaller system matrix which will help numerical accuracy.

Cheers,

Gino

Real-Time Physics Simulation Forum

Johnson's distance algorithm: can't find smallest simplex

Johnson's distance algorithm: can't find smallest simplex

Re: Johnson's distance algorithm: can't find smallest simple

Re: Johnson's distance algorithm: can't find smallest simple

Re: Johnson's distance algorithm: can't find smallest simple

Re: Johnson's distance algorithm: can't find smallest simple