Is there an efficient way to find the nearest line segment to a point in 3 dimensions in python?

Question

I have a point in 3D

p = [0,1,0]  

and a list of line segments defined by their starting and ending co-ordinates.

line_starts = [[1,1,1], [2,2,2], [3,3,3]]  
line_ends = [[5,1,3], [3,2,1], [3, 1, 1]]

I tried adapting the first two algorithms detailed over here in this post: Find the shortest distance between a point and line segments (not line)

But the algorithms are either extremely slow for more than 1k points and line segments or do not work for 3 Dimensions. Is there an efficient way to compute the minimum distance from a point to a line segment, and return the co-ordinates of that point on the line segment?

For e.g. I was able to adapt this code from the post linked above for example, but it is extremely slow.

import math
import numpy as np

def dot(v,w):  
    x,y,z = v  
    X,Y,Z = w   
    return x*X + y*Y + z*Z  

def length(v):  
    x,y,z = v  
    return math.sqrt(x*x + y*y + z*z)  

def vector(b,e):  
    x,y,z = b  
    X,Y,Z = e  
    return (X-x, Y-y, Z-z)  

def unit(v):   
    x,y,z = v  
    mag = length(v)   
    return (x/mag, y/mag, z/mag)  

def distance(p0,p1):  
    return length(vector(p0,p1))  

def scale(v,sc):  
    x,y,z = v  
    return (x * sc, y * sc, z * sc)  

def add(v,w):  
    x,y,z = v  
    X,Y,Z = w  
    return (x+X, y+Y, z+Z)  


'''Given a line with coordinates 'start' and 'end' and the 
coordinates of a point 'pnt' the proc returns the shortest   
distance from pnt to the line and the coordinates of the   
nearest point on the line.  
1  Convert the line segment to a vector ('line_vec').  
2  Create a vector connecting start to pnt ('pnt_vec').  
3  Find the length of the line vector ('line_len').  
4  Convert line_vec to a unit vector ('line_unitvec').  
5  Scale pnt_vec by line_len ('pnt_vec_scaled').  
6  Get the dot product of line_unitvec and pnt_vec_scaled ('t').   
7  Ensure t is in the range 0 to 1.  
8  Use t to get the nearest location on the line to the end  
   of vector pnt_vec_scaled ('nearest').  
9  Calculate the distance from nearest to pnt_vec_scaled.  
10 Translate nearest back to the start/end line.   
Malcolm Kesson 16 Dec 2012'''

def pnt2line(array):  
    pnt = array[0]  
    start = array[1]   
    end = array[2]  
    line_vec = vector(start, end)  
    pnt_vec = vector(start, pnt)  
    line_len = length(line_vec)  
    line_unitvec = unit(line_vec)  
    pnt_vec_scaled = scale(pnt_vec, 1.0/line_len)  
    t = dot(line_unitvec, pnt_vec_scaled)        
    if t < 0.0:  
        t = 0.0  
    elif t > 1.0:  
        t = 1.0  
    nearest = scale(line_vec, t)  
    dist = distance(nearest, pnt_vec)  
    nearest = add(nearest, start)  
    return (round(dist, 3), [round(i, 3) for i in nearest])  



def get_nearest_line(input_d):  
    ''' 
    input_d is an array of arrays  
    Each subarray is [point, line_start, line_end]  
    The point must be the same for all sub_arrays  
    '''   
    op = np.array(list(map(pnt2line, input_d)))  
    ind = np.argmin(op[:, 0])  
    return ind, op[ind, 0], op[ind, 1]  



if __name__ == '__main__':  
    p = [0,1,0]    
  
    line_starts = [[1,1,1], [2,2,2], [3,3,3]]    
    line_ends = [[5,1,3], [3,2,1], [3, 1, 1]]  

    input_d = [[p, line_starts[i], line_ends[i]] for i in range(len(line_starts))]  
    print(get_nearest_line(input_d))  

Output:

(0, 1.414, [1.0, 1.0, 1.0])

Here, (0 - the first line segment was closest,
1.414 - the distance to the line segment,
[1.0, 1.0, 1.0] - the point on the line segment closest to the given point)

The problem is the above code is extremely slow. Further, I have about 10K points, and a fixed set of 10K line segments. For each of the points, I have to find the closest line segment, and the point on the line segment which is the closest. Right now it takes 30 mins to process 10K points.

Is there an efficient way to achieve this?

Answer 1

You can try this:

import numpy as np


def dot(v, w):
    """
    row-wise dot product of 2-dimensional arrays
    """
    return np.einsum('ij,ij->i', v, w)


def closest(line_starts, line_ends, p):
    """
    find line segment closest to the point p 
    """

    # array of vectors from the start to the end of each line segment
    se = line_ends - line_starts
    # array of vectors from the start of each line segment to the point p
    sp = p - line_starts
    # array of vectors from the end of each line segment to p
    ep = p - line_ends

    # orthogonal projection of sp onto se
    proj = (dot(sp, se) / dot(se, se)).reshape(-1, 1) * se
    # orthogonal complement of the projection
    n = sp - proj
    
    # squares of distances from the start of each line segment to p
    starts_d = dot(sp, sp)
    # squares of distances from the end of each line segments to p
    ends_d = dot(ep, ep)
    # squares of distances between p and each line
    lines_d = dot(n, n)

    # If the point determined by the projection is inside
    # the line segment, it is the point of the line segment
    # closest to p; otherwhise the closest point is one of
    # the enpoints. Determine which of these cases holds 
    # and compute the square of the distance to each line segment. 
    coeffs = dot(proj, se)
    dist = np.select([coeffs < 0, coeffs < dot(se, se), True],
                     [starts_d, lines_d, ends_d])

    # find the index of the closest line segment, its distance to p,
    # and the point in this line segment closest to p
    idx = np.argmin(dist)
    min_dist = dist[idx]
    if min_dist == starts_d[idx]:
        min_point = line_starts[idx]
    elif min_dist == ends_d[idx]:
        min_point = line_ends[idx]
    else:
        min_point = line_starts[idx] + proj[idx]
    return idx, min_dist**0.5, min_point

Example:

line_starts = np.array([[1,1,1], [2,2,2], [3,3,3]])  
line_ends = np.array([[5,1,3], [3,2,1], [3, 1, 1]])
p = np.array([0,1,0])

idx, dist, point = closest(line_starts, line_ends, p)
print(f"index = {idx}\ndistance = {dist}\nclosest point = {point}")

It gives:

index = 0
distance = 1.4142135623730951
closest point = [1 1 1]

Since in your case line segments are fixed, and only points are changing, this can be optimized to this situation.