Getting the Closet Factors between Polygons with the GJK Algorithm

I’m making an attempt to implement the GJK algorithm by following this lecture. For essentially the most half, it’s working, however generally 1 of the two closest factors is wrong. Listed here are two examples:

EXAMPLE 1:

The closest factors are accurately calculated.

EXAMPLE 2:

As you may see, the closet level on the sq.’s closest level ought to have been the decrease proper nook and never the higher left nook, and the L-shape’s closet level ought to have been the higher proper nook and never the middle.

I’ve been making an attempt to debug the code for days now, so a recent pair of eyes could be useful. If I did one thing improper, please clarify why; I actually wish to perceive how this all works!

Right here is my code:

public struct Vertex
{
    public float Heart { get; }

    public Vector Level { get; }

    public Vector Point1 { get; }

    public Vector Point2 { get; }

    public Vertex(float heart, Vector level, Vector point1, Vector point2)
    {
        Heart = heart;
        Level = level;
        Point1 = point1;
        Point2 = point2;
    }

    public Vertex(float heart, Vertex vertex)
    {
        Heart = heart;
        Level = vertex.Level;
        Point1 = vertex.Point1;
        Point2 = vertex.Point2;
    }
}

public static class Simplex
{
    non-public static Vector ClosestPoint(Vertex[] simplex)
    {
        change (simplex.Size)
        {
            case 1:
            {
                return simplex[0].Level;
            }
            case 2:
            {
                return ClosestPoint(simplex[0].Level, simplex[1].Level);
            }
            case 3:
            {
                var closetPoint = ClosestPoint(simplex[0].Level, simplex[1].Level);
                var shortestDistance = closetPoint.MagnitudeSquared();

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

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

                    if (distance.IsGreaterThanOrEqualTo(shortestDistance)) proceed;

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

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

    non-public static Vector ClosestPoint(Vector begin, Vector finish)
    {
        var edge = finish - begin;
        var distance = (-start).DotProduct(edge) / edge.MagnitudeSquared();

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

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

        return begin + edge * distance;
    }

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

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

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

            var route = -ClosestPoint(simplex);

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

            var help = SupportPoint(route, polygon, different, out var point1, out var point2);

            if (simplex.Any(vertex => vertex.Level == help))
            {
                break;
            }

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

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

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

            simplex = newSimplex;
        }

        change (simplex.Size)
        {
            case 1:
            {
                return new Level[] {simplex[0].Point1, simplex[0].Point2};
            }
            case 2:
            {
                var scalar = 1.0f / divisor;
                return new Level[]
                {
                    simplex[0].Point1 * (scalar * simplex[0].Heart) + simplex[1].Point1 * (scalar * simplex[1].Heart),
                    simplex[0].Point2 * (scalar * simplex[0].Heart) + simplex[1].Point2 * (scalar * simplex[1].Heart)
                };
            }
            case 3:
            {
                var scalar = 1.0f / divisor;

                return new Level[]
                {
                    simplex[0].Point1 * (scalar * simplex[0].Heart) +
                    simplex[1].Point1 * (scalar * simplex[1].Heart) +
                    simplex[2].Point1 * (scalar * simplex[2].Heart)
                };
            }
            default:
            {
                throw new IndexOutOfRangeException($"The rely is {simplex.Size}, which is out of vary for this operation.");
            }
        }
    }

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

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

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

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

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

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

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

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

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

        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 space = (simplex[1].Level - simplex[0].Level).CrossProduct(simplex[2].Level - simplex[0].Level);

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

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

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

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

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

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

        divisor = space;

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

    non-public static Vector SupportPoint(Vector route, Polygon polygon)
    {
        var supportPoint = polygon.First();
        var supportValue = route.DotProduct(supportPoint);

        for (var index = 1; index < polygon.Depend; index++)
        {
            var worth = route.DotProduct(polygon[index]);

            if (worth.IsLessThanOrEqualTo(supportValue)) proceed;

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

        return supportPoint;
    }

    non-public static Vector SupportPoint(Vector route, Polygon polygon, Polygon different, out Vector point1, out Vector point2)
    {
        point1 = SupportPoint(-direction, polygon);
        point2 = SupportPoint(route, different);
        return point2 - point1;
    }
}

UPDATE:

Out of curiosity, I downloaded the supply code and transformed it from C++ to C#, so apart from minor syntax modifications it was precisely the identical. To my shock the bug was occurring with it as nicely. Does this algorithm not work on concave polygons?

UPDATE 2:

I simply seen it doesn’t work 100% of the time with the sq. and the triangle, so the bug isn’t restricted to concave polygons.

UPDATE 3:

There was a bug with my sq. initialization, forgot to set the purpose rely to 4 so it solely ever took the primary level. Now that it will possibly correctly iterate by the factors it now not is a matter. The L-shape nonetheless has an issue. After researching the algorithm a bit extra, the Minkowski distinction creates a convex hull, and due to this fact is not going to work on concave shapes just like the L. Wanting on the picture now it ought to have been apparent as a result of the purple line ends proper the place the convex hull can be. With all that being stated, is there a special algorithm that I can use or will I simply must iterate over every edge and discover the closest factors that manner?