使用GJK算法获取多边形之间的最近点

Question

我正在通过遵循this讲座来实现GJK算法。在大多数情况下，它可以正常工作，但有时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形仍然存在问题。在对算法进行了更多研究之后，Minkowski差创建了一个凸包，因此不适用于像L那样的凹形。从图像上看，这应该已经很明显了，因为紫色线恰好在凸包所在的位置结束。综上所述，是否可以使用其他算法，还是只需要迭代每个边缘并以这种方式找到最接近的点？