Order a list of points by closest distance

Question

I wrote a function that will take a list of points and then order them so they sort of... "chain together".

The function will start with point #1 in the list.

It will add point #1 to the list of ordered by distance points.

It will then search for the closest point to point #1. (We'll call this point, point #2)

It will then add point #2 to the list of ordered by distance points.

And then... it will search for the closest point to point #2. (Which would be point #3)

You get the point.

The main problem with my code is: it's incredibly slow when dealing with lists that contain tons of points.

I would like some help optimizing my function to make it operate as fast as possible.

private static double Distance(Point p1, Point p2)
{
    return Math.Sqrt(Math.Pow(p2.X - p1.X, 2) + Math.Pow(p2.Y - p1.Y, 2));
}

private List<Point> OrderByDistance(List<Point> pointList)
{
    var orderedList = new List<Point>();

    var currentPoint = pointList[0];

    while (pointList.Count > 1)
    {
        orderedList.Add(currentPoint);
        pointList.RemoveAt(pointList.IndexOf(currentPoint));

        var closestPointIndex = 0;
        var closestDistance = double.MaxValue;

        for (var i = 0; i < pointList.Count; i++)
        {
            var distance = Distance(currentPoint, pointList[i]);
            if (distance < closestDistance)
            {
                closestPointIndex = i;
                closestDistance = distance;
            }
        }

        currentPoint = pointList[closestPointIndex];
    }

    // Add the last point.
    orderedList.Add(currentPoint);

    return orderedList;
}

Answer 1

The main idea is to cover the entire space occupied by points with a rectangular regular grid.

• Each grid cell contains a small subset of points which are located within the cell.

• Since the grid is regular, for a given point we can easily calculate its cell index (I, J).

• Next we search for the nearest point in the range I-1 <= i <= I+1, J-1 <= j <= J+1.

• If no points found, iterate for all indexes in the range I-n <= i <= I+n, J-n <= j <= J+n for n = 2, 3, ..., except indexes from the previous steps.

Side notes:

• There is no need to use the Math.Pow method, consider to use Pow2 method instead:

  private static double Pow2(double x)
  {
      return x * x;
  }
  

• There is no need to calculate distance, consider to use square of distance:

  private static double Distance2(Point p1, Point p2)
  {
      return Pow2(p2.X - p1.X) + Pow2(p2.Y - p1.Y);
  }
  

• There is no need to remove points from the source list, you could iterate

  while (orderedList.Count != pointList.Count)
  

Here is the complete code:

[DebuggerDisplay("X={X}, Y={Y}")]
internal sealed class Point
{
    public readonly double X;
    public readonly double Y;

    public Point(double x, double y)
    {
        X = x;
        Y = y;
    }
}

internal static class PointsSorter
{
    public static List<Point> GeneratePoints(int count)
    {
        Random rnd = new Random();
        List<Point> tmp = new List<Point>(count);
        for (int i = 0; i < count; i++)
        {
            tmp.Add(new Point(rnd.NextDouble() * 100000 - 50000, rnd.NextDouble() * 100000 - 50000));
        }
        return tmp;
    }

    private static double Pow2(double x)
    {
        return x * x;
    }

    private static double Distance2(Point p1, Point p2)
    {
        return Pow2(p2.X - p1.X) + Pow2(p2.Y - p1.Y);
    }

    private static Tuple<Point, double> GetNearestPoint(Point toPoint, LinkedList<Point> points)
    {
        Point nearestPoint = null;
        double minDist2 = double.MaxValue;
        foreach (Point p in points)
        {
            double dist2 = Distance2(p, toPoint);
            if (dist2 < minDist2)
            {
                minDist2 = dist2;
                nearestPoint = p;
            }
        }
        return new Tuple<Point, double>(nearestPoint, minDist2);
    }

    public static List<Point> OrderByDistance(List<Point> points, int gridNx, int gridNy)
    {
        if (points.Count == 0)
            return points;

        double minX = points[0].X;
        double maxX = minX;
        double minY = points[0].Y;
        double maxY = minY;

        // Find the entire space occupied by the points
        foreach (Point p in points)
        {
            double x = p.X;
            double y = p.Y;

            if (x < minX)
                minX = x;
            else if (x > maxX)
                maxX = x;

            if (y < minY)
                minY = y;
            else if (y > maxY)
                maxY = y;
        }

        // The trick to avoid out of range
        maxX += 0.0001;
        maxY += 0.0001;

        double minCellSize2 = Pow2(Math.Min((maxX - minX) / gridNx, (maxY - minY) / gridNy));

        // Create cells subsets
        LinkedList<Point>[,] cells = new LinkedList<Point>[gridNx, gridNy];

        for (int j = 0; j < gridNy; j++)
            for (int i = 0; i < gridNx; i++)
                cells[i, j] = new LinkedList<Point>();

        Func<Point, Tuple<int, int>> getPointIndices = p =>
        {
            int i = (int)((p.X - minX) / (maxX - minX) * gridNx);
            int j = (int)((p.Y - minY) / (maxY - minY) * gridNy);
            return new Tuple<int, int>(i, j);
        };

        foreach (Point p in points)
        {
            var indices = getPointIndices(p);
            cells[indices.Item1, indices.Item2].AddLast(p);
        }

        List<Point> ordered = new List<Point>(points.Count);

        Point nextPoint = points[0];
        while (ordered.Count != points.Count)
        {
            Point p = nextPoint;
            var indices = getPointIndices(p);
            int pi = indices.Item1;
            int pj = indices.Item2;

            ordered.Add(p);
            cells[pi, pj].Remove(p);

            int radius = 1;
            int maxRadius = Math.Max(Math.Max(pi, cells.GetLength(0) - pi), Math.Max(pj, cells.GetLength(1) - pj));

            double[] minDist2 = { double.MaxValue };    // To avoid access to modified closure
            Point nearestPoint = null;

            while ((nearestPoint == null || minDist2[0] > minCellSize2 * (radius - 1)) && radius < maxRadius)
            {
                int minI = Math.Max(pi - radius, 0);
                int minJ = Math.Max(pj - radius, 0);
                int maxI = Math.Min(pi + radius, cells.GetLength(0) - 1);
                int maxJ = Math.Min(pj + radius, cells.GetLength(1) - 1);

                // Find the nearest point in the (i, j)-subset action
                Action<int, int> findAction = (i, j) =>
                {
                    if (cells[i, j].Count != 0)
                    {
                        var areaNearestPoint = GetNearestPoint(p, cells[i, j]);
                        if (areaNearestPoint.Item2 < minDist2[0])
                        {
                            minDist2[0] = areaNearestPoint.Item2;
                            nearestPoint = areaNearestPoint.Item1;
                        }
                    }
                };

                if (radius == 1)
                {
                    // Iterate through all indexes in the 3x3
                    for (int j = minJ; j <= maxJ; j++)
                    {
                        for (int i = minI; i <= maxI; i++)
                        {
                            findAction(i, j);
                        }
                    }
                }
                else
                {
                    // Iterate through border only
                    for (int i = minI; i < maxI; i++)
                    {
                        findAction(i, minJ);
                    }
                    for (int j = minJ; j < maxJ; j++)
                    {
                        findAction(maxI, j);
                    }
                    for (int i = minI + 1; i <= maxI; i++)
                    {
                        findAction(i, maxJ);
                    }
                    for (int j = minJ + 1; j <= maxJ; j++)
                    {
                        findAction(minI, j);
                    }
                }

                radius++;
            }
            nextPoint = nearestPoint;
        }
        return ordered;
    }
}

Usage:

var sortedPoints = PointsSorter.OrderByDistance(PointsSorter.GeneratePoints(500000),
                   500, 500);

Execution time on my PC (in Debug): ~15 seconds.

Answer 2

I don't like any of these answers. In particular, the accepted answer is simply wrong.

Let's start by critiquing the interface:

List<Point> OrderByDistance(List<Point> pointList)

The contract is: the list must contain at least one element, the list is destroyed (!!!) by the method, the first element is special, and the result is a mutable list. I like nothing about any of this; this sounds like a ball of potential bugs. The right contract is:

ImmutableList<Point> OrderByDistance(Point start, ImmutableSet<Point> points)

Look at how many problems this solves. Do we need the set to contain at least one point? No. We already have the start point. Do we destroy the set of points? No. It's immutable. And so on.

Now that we have the signature correct, the algorithm is simple:

ImmutableList<Point> OrderByDistance(Point start, ImmutableSet<Point> points)
{
  var current = start;
  var remaining = points;
  var path = ImmutableList<Point>.Empty.Add(start);
  while(!remaining.IsEmpty)
  {
    var next = Closest(current, remaining);
    path = path.Add(next);
    remaining = remaining.Remove(next);
    current = next;
  }
  return path;
}

Now all you have to do is efficiently implement Closest, which you should be able to do given the previous hints about computing distances cheaply and so on.

If you want a more sophisticated algorithm for Closest, then you need to do some research on this well-studied problem:

https://en.wikipedia.org/wiki/Nearest_neighbor_search

Answer 3

I don't think you get this better than O(N^2) in the worst case, so your solution is optimal in that way.

What you should do is changing the data structure of your point lists, because RemoveAt/IndexOf are O(N). You just waste time, when a linked list could do the same operation in O(1).

Also, Math.Pow(x,2) is probably a bit slower than just x*x. And the square root is altogether superfluous as long as you're interested in the smallest distance only (and not in the absolute value of it).

Answer 4

I think the algorithm supposed by Dmitry will improve the performance...

Additional to the other posts, there is one concrete optimization for your solution:

It is not required to get the index in line pointList.IndexOf(currentPoint) because it is already know:

[...]
var closestPointIndex = 0;
var currentPoint = pointList[closestPointIndex];

while (pointList.Count > 1)
{
    orderedList.Add(currentPoint);
    pointList.RemoveAt(pointList[closestPointIndex]);

    closestPointIndex = 0;
    var closestDistance = double.MaxValue;

    for (var i = 0; i < pointList.Count; i++)
    {
        var distance = Distance(currentPoint, pointList[i]);
        if (distance < closestDistance)
        {
            closestPointIndex = i;
            closestDistance = distance;
        }
    }

    currentPoint = pointList[closestPointIndex];
}
[...]

Answer 5

I would use a cheap a DistanceQuick to dismiss it cheap

Do you really need double? That is expensive.
decimal or float won't do.

Are your points integer? ixi = dxd if d if d%1 = 0;

private static double Distance(Point p1, Point p2)
{
    return Math.Sqrt(Math.Pow(p2.X - p1.X, 2) + Math.Pow(p2.Y - p1.Y, 2));
}
private static double DistanceQuick(Point p1, Point p2)
{
    // distance will this or less
    double deltaX = Math.Abs(p2.X - p1.X);
    double deltaY = Math.Abs(p2.Y - p1.Y);
    return deltaX > deltaY ? deltaX : deltaY;
}    
private List<Point> OrderByDistance(List<Point> pointList)
{
    var orderedList = new List<Point>();
    var currentPoint = pointList[0];
    while (pointList.Count > 1)
    {
        orderedList.Add(currentPoint);
        pointList.RemoveAt(pointList.IndexOf(currentPoint));
        var closestPointIndex = 0;
        var closestDistance = double.MaxValue;
        for (var i = 0; i < pointList.Count; i++)
        {
            var distanceQuick = DistanceQuick(currentPoint, pointList[i]);
            if(distanceQuick > closestDistance)
                continue;
            var distance = Distance(currentPoint, pointList[i]);
            if (distance < closestDistance)
            {
                closestPointIndex = i;
                closestDistance = distance;
            }
        }    
        currentPoint = pointList[closestPointIndex];
    }
    // Add the last point.
    orderedList.Add(currentPoint);
    return orderedList;
}