CODEWITHSUNDEEP
c#unity-game-engine
seek
seek

Reputation: 1105

How to find closest point on multisegment line

I'm trying to find a solution for best performance.

I need to find closet point on multisegment line (List points) to given point.

My line have thousands of points and I need to check distance to this line few times per second. So solution need to be very fast.

Right now I have something like below. It works but it is going to be slow when line have 10000+ points.

Maybe someone have idea how to make it faster?

public static float GetSqrDistXZ(Vector3 a, Vector3 b)
{
    Vector3 vector = new Vector3(a.x - b.x, 0, a.z - b.z);
    return vector.sqrMagnitude;
}

public static Vector3 NearestPointOnFiniteLine(Vector3 start, Vector3 end, Vector3 pnt)
    {
        Vector3 line = (end - start);
        float len =  Mathf.Sqrt(line.sqrMagnitude);
        line.Normalize();

        Vector3 v = pnt - start;
        float d = (v.x * line.x) + (v.z * line.z); 
        d = Mathf.Clamp(d, 0f, len);
        return start + line * d;
    }


int pointsCount = line.points3.Count - 1;  // line - List<Vector3> points.

float[] distances = new float[pointsCount];

    for (int i = 0; i < pointsCount+1; i++) {
        if (i >= 1) {
            distances [i - 1] = GetSqrDistXZ (point, NearestPointOnFiniteLine (line.points3 [i - 1], line.points3 [i], point));
        }

    }

int minListIndexLeft = System.Array.IndexOf (distances, Mathf.Min (distances));
float minimalDistance = distances[minListIndexLeft];

Vector3 closestPoint = NearestPointOnFiniteLine (line.points3[minListIndexLeft], line.points3[minListIndexLeft+1], point);

Upvotes: 0

Views: 590

Answers (1)

Thomas Hilbert
Thomas Hilbert

Reputation: 3629

You'll want to think about space partitioning. In this example I'm going to assume a 2D space, but this works in 3D just as well. Also there are much better solutions like BSP trees and stuff, but we'll keep it simple here.

Imagine putting a grid over your 2D space. Every segment (distance between 2 points) of your line intersects with one or more cells of that grid. What you have to do is to store the intersecting segments for every cell. If your line does not change, you can do that in one single pass on startup, or even store that information statically in an Asset.

But once you have that information, all you have to do is calculate the cell that your point is inside and then only check the line segments that intersect with that specific cell or a number of direct neighbours (see below). This makes finding the closest point lightning fast in comparison.

If you play with this idea on a piece of paper you may come across cases where this solution does not yield the closest point, because it did not consider a neighboring cell that contained a closer point. The easiest way to solve this is the following approach:

1. Find cell C, which is the cell your point is in
2. Let cellRange = 0
3. Let point B be undefined

4. Find closest point P among all segments that intersect cell C and its neighboring cells of range cellRange*
5. If B is the same as newly found point P then P is the solution. You are done.
6. Increase cellRange by 1
7. Let B = P
8. Repeat from step 4

* "neighboring cells of range cellRange" means:
  cellRange 0: only cell C, no neighbours
  cellRange 1: cell C and direct neighbours
  cellRange 2: cell C, direct neighbours and their direct neighbours
  ...

This solution basically checks if increasing the search range improves the solution. As soon as increasing the range did not improve the solution, you found the closest point.

Upvotes: 2

Related Questions