Find the shortest distance between a point and line segments (not line)

Question

I have set of line segments (not lines), (A1, B1), (A2, B2), (A3, B3), where A,B are ending points of the line segment. Each A and B has (x,y) coordinates.

QUESTION: I need to know the shortest distance between point O and line segments as shown in the shown figure implemented in line of codes. The code I can really understand is either pseudo-code or Python.

CODE: I tried to solve the problem with this code, unfortunately, it does not work properly.

def dist(A, B, O):
    A_ = complex(*A)
    B_ = complex(*B)
    O_= complex(*O)
    OA = O_ - A_
    OB = O_ - B_
    return min(OA, OB)
# coordinates are given
A1, B1 = [1, 8], [6,4]
A2, B2 = [3,1], [5,2]
A3, B3 = [2,3], [2, 1]
O = [2, 5]
A = [A1, A2, A3]
B = [B1, B2, B3]
print [ dist(i, j, O)  for i, j in zip(A, B)]

Thanks in advance.

Solution 1

Here is the answer. This code belongs to Malcolm Kesson, the source is here. I provided it before with just link itself and it was deleted by the moderator. I assume that the reason for that is because of not providing the code (as an answer).

import math

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(pnt, start, end):
    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 (dist, nearest)

Solution 2

Rather than using a for loop, you can vectorize these operations and get much better performance. Here is my solution that allows you to compute the distance from a single point to multiple line segments with vectorized computation.

def lineseg_dists(p, a, b):
    """Cartesian distance from point to line segment

    Edited to support arguments as series, from:
    https://stackoverflow.com/a/54442561/11208892

    Args:
        - p: np.array of single point, shape (2,) or 2D array, shape (x, 2)
        - a: np.array of shape (x, 2)
        - b: np.array of shape (x, 2)
    """
    # normalized tangent vectors
    d_ba = b - a
    d = np.divide(d_ba, (np.hypot(d_ba[:, 0], d_ba[:, 1])
                           .reshape(-1, 1)))

    # signed parallel distance components
    # rowwise dot products of 2D vectors
    s = np.multiply(a - p, d).sum(axis=1)
    t = np.multiply(p - b, d).sum(axis=1)

    # clamped parallel distance
    h = np.maximum.reduce([s, t, np.zeros(len(s))])

    # perpendicular distance component
    # rowwise cross products of 2D vectors  
    d_pa = p - a
    c = d_pa[:, 0] * d[:, 1] - d_pa[:, 1] * d[:, 0]

    return np.hypot(h, c)

And some tests:

p = np.array([0, 0])
a = np.array([[ 1,  1],
              [-1,  0],
              [-1, -1]])
b = np.array([[ 2,  2],
              [ 1,  0],
              [ 1, -1]])

print(lineseg_dists(p, a, b))

p = np.array([[0, 0],
              [1, 1],
              [0, 2]])

print(lineseg_dists(p, a, b))

>>> [1.41421356 0.         1.        ]
    [1.41421356 1.         3.        ]

Solution 3

Basic algorithm: pretend that you have lines, so oriented that A lies to the left of B when O lies above the line (mentally rotate the picture to match as needed).

Find closest point as normal. If the point is between A and B, you're done. If it's to the left of A, the closest point is A. If the point is to the right of B, the closest point is B.

The case when A, B, and O all lie on the same line may or may not need special attention. Be sure to include a few tests of this position.

Solution 4

The explanation is in the docstring of this function:

def point_to_line_dist(point, line):
    """Calculate the distance between a point and a line segment.

    To calculate the closest distance to a line segment, we first need to check
    if the point projects onto the line segment.  If it does, then we calculate
    the orthogonal distance from the point to the line.
    If the point does not project to the line segment, we calculate the 
    distance to both endpoints and take the shortest distance.

    :param point: Numpy array of form [x,y], describing the point.
    :type point: numpy.core.multiarray.ndarray
    :param line: list of endpoint arrays of form [P1, P2]
    :type line: list of numpy.core.multiarray.ndarray
    :return: The minimum distance to a point.
    :rtype: float
    """
    # unit vector
    unit_line = line[1] - line[0]
    norm_unit_line = unit_line / np.linalg.norm(unit_line)

    # compute the perpendicular distance to the theoretical infinite line
    segment_dist = (
        np.linalg.norm(np.cross(line[1] - line[0], line[0] - point)) /
        np.linalg.norm(unit_line)
    )

    diff = (
        (norm_unit_line[0] * (point[0] - line[0][0])) + 
        (norm_unit_line[1] * (point[1] - line[0][1]))
    )

    x_seg = (norm_unit_line[0] * diff) + line[0][0]
    y_seg = (norm_unit_line[1] * diff) + line[0][1]

    endpoint_dist = min(
        np.linalg.norm(line[0] - point),
        np.linalg.norm(line[1] - point)
    )

    # decide if the intersection point falls on the line segment
    lp1_x = line[0][0]  # line point 1 x
    lp1_y = line[0][1]  # line point 1 y
    lp2_x = line[1][0]  # line point 2 x
    lp2_y = line[1][1]  # line point 2 y
    is_betw_x = lp1_x <= x_seg <= lp2_x or lp2_x <= x_seg <= lp1_x
    is_betw_y = lp1_y <= y_seg <= lp2_y or lp2_y <= y_seg <= lp1_y
    if is_betw_x and is_betw_y:
        return segment_dist
    else:
        # if not, then return the minimum distance to the segment endpoints
        return endpoint_dist

Solution 5

On my side, I've found that the two other answers were broken, especially when the line is purely vertical or horizontal. Here is what I did to properly solve the problem.

Python code:

def sq_shortest_dist_to_point(self, other_point):
    dx = self.b.x - self.a.x
    dy = self.b.y - self.a.y
    dr2 = float(dx ** 2 + dy ** 2)

    lerp = ((other_point.x - self.a.x) * dx + (other_point.y - self.a.y) * dy) / dr2
    if lerp < 0:
        lerp = 0
    elif lerp > 1:
        lerp = 1

    x = lerp * dx + self.a.x
    y = lerp * dy + self.a.y

    _dx = x - other_point.x
    _dy = y - other_point.y
    square_dist = _dx ** 2 + _dy ** 2
    return square_dist

def shortest_dist_to_point(self, other_point):
    return math.sqrt(self.sq_shortest_dist_to_point(other_point))

A test case:

def test_distance_to_other_point(self):
    # Parametrize test with multiple cases:
    segments_and_point_and_answer = [
        [Segment(Point(1.0, 1.0), Point(1.0, 3.0)), Point(2.0, 4.0), math.sqrt(2.0)],
        [Segment(Point(1.0, 1.0), Point(1.0, 3.0)), Point(2.0, 3.0), 1.0],
        [Segment(Point(0.0, 0.0), Point(0.0, 3.0)), Point(1.0, 1.0), 1.0],
        [Segment(Point(1.0, 1.0), Point(3.0, 3.0)), Point(2.0, 2.0), 0.0],
        [Segment(Point(-1.0, -1.0), Point(3.0, 3.0)), Point(2.0, 2.0), 0.0],
        [Segment(Point(1.0, 1.0), Point(1.0, 3.0)), Point(2.0, 3.0), 1.0],
        [Segment(Point(1.0, 1.0), Point(1.0, 3.0)), Point(2.0, 4.0), math.sqrt(2.0)],
        [Segment(Point(1.0, 1.0), Point(-3.0, -3.0)), Point(-3.0, -4.0), 1],
        [Segment(Point(1.0, 1.0), Point(-3.0, -3.0)), Point(-4.0, -3.0), 1],
        [Segment(Point(1.0, 1.0), Point(-3.0, -3.0)), Point(1, 2), 1],
        [Segment(Point(1.0, 1.0), Point(-3.0, -3.0)), Point(2, 1), 1],
        [Segment(Point(1.0, 1.0), Point(-3.0, -3.0)), Point(-3, -1), math.sqrt(2.0)],
        [Segment(Point(1.0, 1.0), Point(-3.0, -3.0)), Point(-1, -3), math.sqrt(2.0)],
        [Segment(Point(-1.0, -1.0), Point(3.0, 3.0)), Point(3, 1), math.sqrt(2.0)],
        [Segment(Point(-1.0, -1.0), Point(3.0, 3.0)), Point(1, 3), math.sqrt(2.0)],
        [Segment(Point(1.0, 1.0), Point(3.0, 3.0)), Point(3, 1), math.sqrt(2.0)],
        [Segment(Point(1.0, 1.0), Point(3.0, 3.0)), Point(1, 3), math.sqrt(2.0)]
    ]

    for i, (segment, point, answer) in enumerate(segments_and_point_and_answer):
        result = segment.shortest_dist_to_point(point)
        self.assertAlmostEqual(result, answer, delta=0.001, msg=str((i, segment, point, answer)))

Note: I assume this function is inside a Segment class. In case your line is infinite, don't limit the lerp from 0 to 1 only, but still at least provide two distinct a and b points.

View more solutions

Comments

[Reticulated Spline]

Please post what you have tried so far.

[Spider]

@ReticulatedSpline. I have added some codes that I have tried to solve the problem. What would you think ?

[Spider]

Thank you very much. The problem is I am a bit far away from mathematics. and I am unable to move in terms of coding (preferably Python). Can you give me a lift?

[9000]

I'm sure you'll be able to code this formula. Ordering of points and edge cases are still on you. Sorry, this site is generally not about writing code for you, unless you have written enough code yourself, though exceptions happen.

[Spider]

Great asset. I really appreciate, much better and shorter case. I hope it includes all tricky exceptional cases..!

[9000]

One more hint: if a point N lies between two other points A and B, each coordinate of N lies between corresponding coordinates of A and B. If the closest point is not between A and B, you can just calculate (squares of) distances form point O to A, then to B, and pick the shortest.

[Spider]

I did that, but can you look at what I have posted below in the link. I guess all cases are included in this short code. And as a specialist, can you confirm that it works in all cases(Python code)? Great thanks

[Roman]

Your maths is broken. Do some tests with different lines and points

[Roman]

I've taken your template and fixed it so that no artificial constant needs to be added, and also inline values are now correct

[Pranav Nutalapati]

No explanation. Please do not post code-only answers.

[schapke]

The answer is incorrect. Demonstration by drawing: imgur.com/a/JAZYniV

[9000]

@schapke: Thank you for pointing this out; indeed for "left" and "right" to work as intended, O needs to be placed "above". I'll update the formulation.

[schapke]

@9000 When O is not between AB in either the y or x axis then the closest point will be either A or B, it's easier in that case just to check both the distances to both points.

[9000]

@schapke: if you have prior knowledge like this, then yes; in the general case finding the projection of O is the fist step, and with the projection, you don't need multiplication to find the closest point if O is not between A and B.

[Realhermit]

How do I adapt this to 3 dimensions?