Получение ближайших точек между полигонами с помощью алгоритма GJK

Question

Я пытаюсь реализовать алгоритм GJK, следуя этой лекции.По большей части это работает, но иногда 1 из 2 ближайших пунктов неверен.Вот два примера:

ПРИМЕР 1:

Ближайшие точки рассчитаны правильно.

ПРИМЕР 2:

Как видите, точка закрытия на ближайшей точке квадрата должна была находиться в правом нижнем углуи не верхний левый угол, и точка шкафа L-формы должна была быть верхним правым углом, а не центром.

Я уже несколько дней пытаюсь отладить код, поэтому может пригодиться свежая пара глаз.Если я сделал что-то не так, пожалуйста, объясните, почему;Я очень хочу понять, как все это работает!

Вот мой код:

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

ОБНОВЛЕНИЕ:

Из любопытства я скачалисходный код и преобразовал его из C ++ в C #, поэтому, кроме незначительных изменений синтаксиса, он был точно таким же.К моему удивлению, ошибка произошла и с ним.Этот алгоритм не работает на вогнутых многоугольниках?

ОБНОВЛЕНИЕ 2:

Я только что заметил, что он не работает в 100% случаев с квадратом и треугольником, поэтомуошибка не ограничивается вогнутыми полигонами.

ОБНОВЛЕНИЕ 3:

Произошла ошибка с моей квадратной инициализацией, забыл установить счетчик точек на 4, поэтому он толькокогда-либо взял первый пункт.Теперь, когда он может правильно перебирать точки, он больше не является проблемой.У L-формы все еще есть проблема.После более подробного изучения алгоритма разность Минковского создает выпуклую оболочку и, следовательно, не будет работать с вогнутыми формами, такими как L. Глядя на изображение сейчас, это должно было быть очевидно, потому что фиолетовая линия заканчивается там, где выпуклая оболочка будет,С учетом всего сказанного, есть ли другой алгоритм, который я могу использовать, или мне нужно будет просто перебрать каждое ребро и найти самые близкие точки таким образом?