C# 凹壳实现

我正在尝试实现下面描述的算法

我正在使用以下类库。Loyc LIB来自

这是基础课

public class Hulls
{
    private static List<Point<double>> KNearestNeighbors(List<Point<double>> points, Point<double> currentPoint, int k, out int kk)
    {
        kk = Math.Min(k, points.Count - 1);
        var ret = points
            .OrderBy(x => PointMath.Length(new Vector<double>(currentPoint.X - x.X, currentPoint.Y - x.Y)))
            .Take(k)
            .ToList();
        return ret;
    }
    private static double Angle(Point<double> a, Point<double> b)
    {
        var ret = -Math.Atan2(b.Y - a.Y, b.X - a.X);
        return NormaliseAngle(ret);
    }
    private static double NormaliseAngle(double a)
    {
        //while (a < b - Math.PI) a += Math.PI * 2;
        //while (b < a - Math.PI) b += Math.PI * 2;
        if (a < 0.0) { return a + Math.PI + Math.PI; }
        return a;
    }
    private static List<Point<double>> SortByAngle(List<Point<double>> kNearest, Point<double> currentPoint, double angle)
    {
        //kNearest
        //    .Sort((v1, v2) => AngleDifference(angle, Angle(currentPoint, v1)).CompareTo(AngleDifference(angle, Angle(currentPoint, v2))));
        //return kNearest.ToList();
        kNearest = kNearest.OrderByDescending(x => NormaliseAngle(Angle(currentPoint, x) - angle)).ToList();
        return kNearest;
    }

    private static bool CCW(Point<double> p1, Point<double> p2, Point<double> p3)
    {
        var cw = ((p3.Y - p1.Y) * (p2.X - p1.X)) - ((p2.Y - p1.Y) * (p3.X - p1.X));
        return cw > 0 ? true : cw < 0 ? false : true; // colinear 
    }

    private static bool _Intersect(LineSegment<double> seg1, LineSegment<double> seg2)
    {
        return CCW(seg1.A, seg2.A, seg2.B) != CCW(seg1.B, seg2.A, seg2.B) && CCW(seg1.A, seg1.B, seg2.A) != CCW(seg1.A, seg1.B, seg2.B);
    }

    private static bool Intersect(LineSegment<double> seg1, LineSegment<double> seg2)
    {
        if ((seg1.A.X == seg2.A.X && seg1.A.Y == seg2.A.Y) 
            || (seg1.B.X == seg2.B.X && seg1.B.Y == seg2.B.Y))
        {
            return false;
        }
        if (_Intersect(seg1, seg2))
        {
            return true;
        }
        return false;
    }

    public IListSource<Point<double>> KNearestConcaveHull(List<Point<double>> points, int k)
    {
        points.Sort((a, b) => a.Y == b.Y ? (a.X > b.X ? 1 : -1) : (a.Y >= b.Y ? 1 : -1));
        Console.WriteLine("Starting with size {0}", k.ToString());

        DList<Point<double>> hull = new DList<Point<double>>();
        var len = points.Count;

        if (len < 3) { return null; }
        if (len == 3) { return hull; }

        var kk = Math.Min(Math.Max(k, 3), len);

        var dataset = new List<Point<double>>();
        dataset.AddRange(points.Distinct());

        var firstPoint = dataset[0];
        hull.PushFirst(firstPoint);

        var currentPoint = firstPoint;
        dataset.RemoveAt(0);

        double previousAngle = 0;
        int step = 2;
        int i;
        while ((currentPoint != firstPoint || step == 2) && dataset.Count > 0)
        {
            if (step == 5) { dataset.Add(firstPoint); }
            var kNearest = KNearestNeighbors(dataset, currentPoint, k, out kk);
            var cPoints = SortByAngle(kNearest, currentPoint, previousAngle);
            var its = true;
            i = 0;
            while (its == true && i < cPoints.Count)
            {
                i++;
                int lastPoint = 0;
                if (cPoints[i - 1] == firstPoint)
                {
                    lastPoint = 1;
                }
                int j = 2;
                its = false;
                while (its == false && j < hull.Count - lastPoint)
                {
                    LineSegment<double> line1 = new LineSegment<double>(hull[step - 2], cPoints[i - 1]);
                    LineSegment<double> line2 = new LineSegment<double>(hull[step - 2 - j], hull[step - 1 - j]);

                    //its = LineMath.ComputeIntersection(line1, line2, out pfrac, LineType.Segment);
                    its = Intersect(line1, line2);
                    j++;
                }
            }
            if (its == true)
            {
                return KNearestConcaveHull(points, kk + 1);
            }
            currentPoint = cPoints[i - 1];
            hull.PushLast(currentPoint);
            previousAngle = Angle(hull[step - 1], hull[step - 2]);
            dataset.Remove(currentPoint); 
            step++;
        }
        bool allInside = true;
        i = dataset.Count;
        while (allInside == true && i > 0)
        {
            allInside = PolygonMath.IsPointInPolygon(hull, dataset[i - 1]);
            i--;
        }
        if (allInside == false) { return KNearestConcaveHull(points, kk + 1); }
        return hull;
    }
}

公共类外壳
{
专用静态列表KNearestNeighbors（列表点、点currentPoint、int k、out int kk）
{
kk=Math.Min（k，points.Count-1）；
var ret=点数
.OrderBy（x=>PointMath.Length（新向量（currentPoint.x-x.x，currentPoint.Y-x.Y）））
.Take（k）
.ToList（）；
返回ret；
}
专用静态双角度（点a、点b）
{
var ret=-Math.Atan2（b.Y-a.Y，b.X-a.X）；
返回法线角度（ret）；
}
专用静态双法线角度（双a）
{
//而（a角度差（角度，角度（当前点，v1））。比较（角度差（角度，角度（当前点，v2）））；
//返回kNearest.ToList（）；
kNearest=kNearest.OrderByDescending（x=>NormaliseAngle（角度（当前点，x）-角度））.ToList（）；
回揉；
}
专用静态bool CCW（点p1、点p2、点p3）
{
var cw=（（p3.Y-p1.Y）*（p2.X-p1.X））-（（p2.Y-p1.Y）*（p3.X-p1.X））；
返回cw>0？真：cw<0？假：真；//共线
}
专用静态布尔交叉（线段seg1、线段seg2）
{
返回CCW（seg1.A，seg2.A，seg2.B）！=CCW（seg1.B，seg2.A，seg2.B）和&CCW（seg1.A，seg1.B，seg2.A）！=CCW（seg1.A，seg1.B，seg2.B）；
}
专用静态布尔相交（线段seg1、线段seg2）
{
if（（seg1.A.X==seg2.A.X&&seg1.A.Y==seg2.A.Y）
||（seg1.B.X==seg2.B.X&&seg1.B.Y==seg2.B.Y））
{
返回false；
}
如果（_相交（seg1，seg2））
{
返回true；
}
返回false；
}
公共IListSource KNearestConcaveHull（列表点，整数k）
{
点排序（（a，b）=>a.Y==b.Y？（a.X>b.X？1:-1）：（a.Y>=b.Y？1:-1））；
WriteLine（“从大小{0}开始”，k.ToString（））；
DList外壳=新的DList（）；
var len=点数。计数；
if（len<3）{返回null；}
如果（len==3）{返回外壳；}
var kk=Math.Min（Math.Max（k，3），len）；
var数据集=新列表（）；
dataset.AddRange（points.Distinct（））；
var firstPoint=数据集[0]；
船体。推压优先（第一点）；
var currentPoint=第一个点；
dataset.RemoveAt（0）；
双前向角=0；
int步=2；
int i；
while（（currentPoint！=firstPoint | | step==2）和&dataset.Count>0）
{
如果（步骤==5）{dataset.Add（firstPoint）；}
var kNearest=KNearestNeighbors（数据集，当前点，k，out kk）；
var cPoints=SortByAngle（kNearest、currentPoint、previousAngle）；
var-its=true；
i=0；
while（its==true&&i0）
{
allInside=PolygonMath.IsPointInPolygon（hull，数据集[i-1]）；
我--；
}
如果（allInside==false）{返回KNearestConcaveHull（点，kk+1）；}
返回船体；
}
}

假设上面的操作是基于从上一条边沿逆时针方向围绕点集的最远右转弯为边界拾取一条新边。代码似乎从y值最低的初始顶点拾取正确的第一条边，但当偏移角度不为零时，无法正确拾取下一条边。我认为问题在于卑鄙的行为或角度-atan2将返回顺时针旋转，对吗？可能我应该添加偏移角度

编辑（解决方案）：遵循Eric在问题的第一条评论中提供的有用建议后发现问题。这是一个肮脏的天使：

private static double Angle(Point<double> a, Point<double> b)
    {
        var ret = Math.Atan2(b.Y - a.Y, b.X - a.X);
        return NormaliseAngle(ret);
    }

    private static double NormaliseAngle(double a)
    {
        if (a < 0.0) { return a + Math.PI + Math.PI; }
        return a;
    }

    private static List<Point<double>> SortByAngle(List<Point<double>> kNearest, Point<double> currentPoint, double angle)
    {
        //kNearest = kNearest.OrderByDescending(x => NormaliseAngle(Angle(currentPoint, x) - angle)).ToList();
        kNearest.Sort((a, b) => NormaliseAngle(Angle(currentPoint, a) - angle) > NormaliseAngle(Angle(currentPoint, b) - angle) ? 1 : -1);
        return kNearest;
    }

专用静态双角度（点a、点b）
{
var ret=数学Atan2（b.Y-a.Y，b.X-a.X）；
返回法线角度（ret）；
}
专用静态双法线角度（双a）
{
如果（a<0.0）{返回a+Math.PI+Math.PI；}
返回a；
}
私有静态列表SortByAngle（列表kNearest、点currentPoint、双角度）
{
//kNearest=kNearest.OrderByDescending（x=>NormaliseAngle（角度（当前点，x）-角度））.ToList（）；
kNearest.Sort（（a，b）=>NormaliseAngle（角度（当前点，a）-角度）>NormaliseAngle（角度（当前点，b）-角度）？1:-1）；
回揉；
private static double Angle(Point<double> a, Point<double> b)
    {
        var ret = Math.Atan2(b.Y - a.Y, b.X - a.X);
        return NormaliseAngle(ret);
    }

    private static double NormaliseAngle(double a)
    {
        if (a < 0.0) { return a + Math.PI + Math.PI; }
        return a;
    }

    private static List<Point<double>> SortByAngle(List<Point<double>> kNearest, Point<double> currentPoint, double angle)
    {
        //kNearest = kNearest.OrderByDescending(x => NormaliseAngle(Angle(currentPoint, x) - angle)).ToList();
        kNearest.Sort((a, b) => NormaliseAngle(Angle(currentPoint, a) - angle) > NormaliseAngle(Angle(currentPoint, b) - angle) ? 1 : -1);
        return kNearest;
    }

var kNearest = KNearestNeighbors(dataset, currentPoint, k, out kk);

int someVal;    
var kNearest = KNearestNeighbors(dataset, currentPoint, k, out someVal);