Skip to content

Instantly share code, notes, and snippets.

@nim65s
Created January 5, 2015 12:57
Show Gist options
  • Star 6 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save nim65s/5e9902cd67f094ce65b0 to your computer and use it in GitHub Desktop.
Save nim65s/5e9902cd67f094ce65b0 to your computer and use it in GitHub Desktop.
distance from point to line segment…
from numpy import arccos, array, dot, pi
from numpy.linalg import det, norm
def distance(A, B, P):
""" segment line AB, point P, where each one is an array([x, y]) """
if all(A == P) or all(B == P):
return 0
if arccos(dot((P - A) / norm(P - A), (B - A) / norm(B - A))) > pi / 2:
return norm(P - A)
if arccos(dot((P - B) / norm(P - B), (A - B) / norm(A - B))) > pi / 2:
return norm(P - B)
return abs(dot(A - B, P[::-1]) + det([A, B])) / norm(A - B)
@peteflorence
Copy link

The last return on line 12 was wrong. This works instead:

from numpy import arccos, array, dot, pi, cross
from numpy.linalg import det, norm

# from: https://gist.github.com/nim65s/5e9902cd67f094ce65b0
def distance_numpy(A, B, P):
    """ segment line AB, point P, where each one is an array([x, y]) """
    if all(A == P) or all(B == P):
        return 0
    if arccos(dot((P - A) / norm(P - A), (B - A) / norm(B - A))) > pi / 2:
        return norm(P - A)
    if arccos(dot((P - B) / norm(P - B), (A - B) / norm(A - B))) > pi / 2:
        return norm(P - B)
    return norm(cross(A-B, A-P))/norm(B-A)

@Ashlanfox
Copy link

Ashlanfox commented Dec 12, 2020

The last return on line 12 was wrong. This works instead:

from numpy import arccos, array, dot, pi, cross
from numpy.linalg import det, norm

# from: https://gist.github.com/nim65s/5e9902cd67f094ce65b0
def distance_numpy(A, B, P):
    """ segment line AB, point P, where each one is an array([x, y]) """
    if all(A == P) or all(B == P):
        return 0
    if arccos(dot((P - A) / norm(P - A), (B - A) / norm(B - A))) > pi / 2:
        return norm(P - A)
    if arccos(dot((P - B) / norm(P - B), (A - B) / norm(A - B))) > pi / 2:
        return norm(P - B)
    return norm(cross(A-B, A-P))/norm(B-A)

You did not handle the case where A equals B, which throws a true_divide. Something like this should be added:
if all(A == B): return dist(A, P)

I would also add a np.clip within arccos such that: arccos(np.clip(angle, -1, 1))

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment