1
\$\begingroup\$

I am trying to implement the GJK algorithm by following this lecture. For the most part, it is working, but sometimes 1 of the 2 closest points is incorrect. Here are two examples:

EXAMPLE 1:

The closest points are correctly calculated. Example1

EXAMPLE 2:

As you can see, the closet point on the square's closest point should have been the lower right corner and not the upper left corner, and the L-shape's closet point should have been the upper right corner and not the center. Example2

I have been trying to debug the code for days now, so a fresh pair of eyes might be helpful. If I did something wrong, please explain why; I really want to understand how this all works!

Here is my code:

public struct Vertex
{
    public float Center { get; }

    public Vector Point { get; }

    public Vector Point1 { get; }

    public Vector Point2 { get; }

    public Vertex(float center, Vector point, Vector point1, Vector point2)
    {
        Center = center;
        Point = point;
        Point1 = point1;
        Point2 = point2;
    }

    public Vertex(float center, Vertex vertex)
    {
        Center = center;
        Point = vertex.Point;
        Point1 = vertex.Point1;
        Point2 = vertex.Point2;
    }
}

public static class Simplex
{
    private static Vector ClosestPoint(Vertex[] simplex)
    {
        switch (simplex.Length)
        {
            case 1:
            {
                return simplex[0].Point;
            }
            case 2:
            {
                return ClosestPoint(simplex[0].Point, simplex[1].Point);
            }
            case 3:
            {
                var closetPoint = ClosestPoint(simplex[0].Point, simplex[1].Point);
                var shortestDistance = closetPoint.MagnitudeSquared();

                var points = new[]
                {
                    ClosestPoint(simplex[1].Point, simplex[2].Point),
                    ClosestPoint(simplex[2].Point, simplex[0].Point)
                };

                for (var index = 0; index < points.Length; index++)
                {
                    var distance = points[index].MagnitudeSquared();

                    if (distance.IsGreaterThanOrEqualTo(shortestDistance)) continue;

                    closetPoint = points[index];
                    shortestDistance = distance;
                }

                return closetPoint;
            }
            default:
            {
                throw new IndexOutOfRangeException($"The count is {simplex.Length}, which is out of range for this operation.");
            }
        }
    }

    private static Vector ClosestPoint(Vector start, Vector end)
    {
        var edge = end - start;
        var distance = (-start).DotProduct(edge) / edge.MagnitudeSquared();

        if (distance.IsLessThanZero())
        {
            return start;
        }

        if (distance.IsGreaterThan(1.0f))
        {
            return end;
        }

        return start + edge * distance;
    }

    public static Point[] Solve(Polygon polygon, Polygon other)
    {
        var divisor = 1.0f;
        var simplex = new[] {new Vertex(1.0f, (Vector) other.First() - polygon.First(), polygon.First(), other.First())};

        for (var iteration = 0; iteration < 20; iteration++)
        {
            switch (simplex.Length)
            {
                case 1:
                {
                    break;
                }
                case 2:
                {
                    simplex = OneSimplex(simplex, out divisor);
                    break;
                }
                case 3:
                {
                    simplex = TwoSimplex(simplex, out divisor);
                    break;
                }
                default:
                {
                    throw new IndexOutOfRangeException($"The count is {simplex.Length}, which is out of range for this operation.");
                }
            }

            if (simplex.Length == 3)
            {
                break;
            }

            var direction = -ClosestPoint(simplex);

            if (direction.DotProduct(direction).IsZero())
            {
                break;
            }

            var support = SupportPoint(direction, polygon, other, out var point1, out var point2);

            if (simplex.Any(vertex => vertex.Point == support))
            {
                break;
            }

            var newSimplex = new Vertex[simplex.Length + 1];

            for (var index = 0; index < simplex.Length; index++)
            {
                newSimplex[index] = simplex[index];
            }

            newSimplex[simplex.Length] = new Vertex(1.0f, support, point1, point2);

            simplex = newSimplex;
        }

        switch (simplex.Length)
        {
            case 1:
            {
                return new Point[] {simplex[0].Point1, simplex[0].Point2};
            }
            case 2:
            {
                var scalar = 1.0f / divisor;
                return new Point[]
                {
                    simplex[0].Point1 * (scalar * simplex[0].Center) + simplex[1].Point1 * (scalar * simplex[1].Center),
                    simplex[0].Point2 * (scalar * simplex[0].Center) + simplex[1].Point2 * (scalar * simplex[1].Center)
                };
            }
            case 3:
            {
                var scalar = 1.0f / divisor;

                return new Point[]
                {
                    simplex[0].Point1 * (scalar * simplex[0].Center) +
                    simplex[1].Point1 * (scalar * simplex[1].Center) +
                    simplex[2].Point1 * (scalar * simplex[2].Center)
                };
            }
            default:
            {
                throw new IndexOutOfRangeException($"The count is {simplex.Length}, which is out of range for this operation.");
            }
        }
    }

    private static Vertex[] OneSimplex(Vertex[] simplex, out float divisor)
    {
        var v = (-simplex[0].Point).DotProduct(simplex[1].Point - simplex[0].Point);

        if (v.IsLessThanZero())
        {
            divisor = 1.0f;
            return new[] {new Vertex(1.0f, simplex[0])};
        }

        var u = (-simplex[1].Point).DotProduct(simplex[0].Point - simplex[1].Point);

        if (u.IsLessThanZero())
        {
            divisor = 1.0f;
            return new[] {new Vertex(1.0f, simplex[1])};
        }

        var edge = simplex[1].Point - simplex[0].Point;

        divisor = edge.DotProduct(edge);
        return new[] {new Vertex(u, simplex[0]), new Vertex(v, simplex[1])};
    }

    private static Vertex[] TwoSimplex(Vertex[] simplex, out float divisor)
    {
        var uAb = (-simplex[1].Point).DotProduct(simplex[0].Point - simplex[1].Point);
        var vAb = (-simplex[0].Point).DotProduct(simplex[1].Point - simplex[0].Point);

        var uBc = (-simplex[2].Point).DotProduct(simplex[1].Point - simplex[2].Point);
        var vBc = (-simplex[1].Point).DotProduct(simplex[2].Point - simplex[1].Point);

        var uCa = (-simplex[0].Point).DotProduct(simplex[2].Point - simplex[0].Point);
        var vCa = (-simplex[2].Point).DotProduct(simplex[0].Point - simplex[2].Point);

        if (vAb.IsLessThanOrEqualToZero() && uCa.IsLessThanOrEqualToZero())
        {
            divisor = 1.0f;
            return new[] {new Vertex(1.0f, simplex[0])};
        }

        if (uAb.IsLessThanOrEqualToZero() && vBc.IsLessThanOrEqualToZero())
        {
            divisor = 1.0f;
            return new[] {new Vertex(1.0f, simplex[1])};
        }

        if (uBc.IsLessThanOrEqualToZero() && vCa.IsLessThanOrEqualToZero())
        {
            divisor = 1.0f;
            return new[] {new Vertex(1.0f, simplex[2])};
        }

        var area = (simplex[1].Point - simplex[0].Point).CrossProduct(simplex[2].Point - simplex[0].Point);

        var uAbc = (simplex[1].Point).CrossProduct(simplex[2].Point);
        var vAbc = (simplex[2].Point).CrossProduct(simplex[0].Point);
        var wAbc = (simplex[0].Point).CrossProduct(simplex[1].Point);

        if (uAb.IsGreaterThanZero() && vAb.IsGreaterThanZero() && (wAbc * area).IsLessThanOrEqualToZero())
        {
            var edge = simplex[1].Point - simplex[0].Point;

            divisor = edge.DotProduct(edge);
            return new[] {new Vertex(uAb, simplex[0]), new Vertex(vAb, simplex[1])};
        }

        if (uBc.IsGreaterThanZero() && vBc.IsGreaterThanZero() && (uAbc * area).IsLessThanOrEqualToZero())
        {
            var edge = simplex[2].Point - simplex[1].Point;
            divisor = edge.DotProduct(edge);
            return new[] {new Vertex(uBc, simplex[1]), new Vertex(vBc, simplex[2])};
        }

        if (uCa.IsGreaterThanZero() && vCa.IsGreaterThanZero() && (vAbc * area).IsLessThanOrEqualToZero())
        {
            var edge = simplex[0].Point - simplex[2].Point;

            divisor = edge.DotProduct(edge);
            return new[] {new Vertex(uCa, simplex[2]), new Vertex(vCa, simplex[0])};
        }

        divisor = area;

        return new[] {new Vertex(uAbc, simplex[0]), new Vertex(vAbc, simplex[1]), new Vertex(wAbc, simplex[2])};
    }

    private static Vector SupportPoint(Vector direction, Polygon polygon)
    {
        var supportPoint = polygon.First();
        var supportValue = direction.DotProduct(supportPoint);

        for (var index = 1; index < polygon.Count; index++)
        {
            var value = direction.DotProduct(polygon[index]);

            if (value.IsLessThanOrEqualTo(supportValue)) continue;

            supportPoint = polygon[index];
            supportValue = value;
        }

        return supportPoint;
    }

    private static Vector SupportPoint(Vector direction, Polygon polygon, Polygon other, out Vector point1, out Vector point2)
    {
        point1 = SupportPoint(-direction, polygon);
        point2 = SupportPoint(direction, other);
        return point2 - point1;
    }
}

UPDATE:

Out of curiosity, I downloaded the source code and converted it from C++ to C#, so besides minor syntax changes it was exactly the same. To my surprise the bug was happening with it as well. Does this algorithm not work on concave polygons?

UPDATE 2:

I just noticed it does not work 100% of the time with the square and the triangle, so the bug is not limited to concave polygons.

UPDATE 3:

There was a bug with my square initialization, forgot to set the point count to 4 so it only ever took the first point. Now that it can properly iterate through the points it no longer is an issue. The L-shape still has a problem. After researching the algorithm a bit more, the Minkowski difference creates a convex hull, and therefore will not work on concave shapes like the L. Looking at the image now it should have been obvious because the purple line ends right where the convex hull would be. With all that being said, is there a different algorithm that I can use or will I just have to iterate over each edge and find the closest points that way?

\$\endgroup\$
  • \$\begingroup\$ Have you solved your original issue now? I think you do need to iterate each edge to find the closest point. \$\endgroup\$ – Jay Feb 12 at 3:49
  • \$\begingroup\$ You may be right, but I am going to try partitioning each polygon into convex sub-parts and then try running this algorithm on that. I don't think there would be a performance hit doing it edge by edge on squares or pentagons, but 20+ edges would result in hundreds of checks. \$\endgroup\$ – Jedi_Maseter_Sam Feb 12 at 16:15

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Browse other questions tagged or ask your own question.