CODEWITHSUNDEEP
pythonalgorithmnumpygeometry
Andrii Syd
Andrii Syd

Reputation: 328

How to find nearest point in segment in a 3d space

I am solving an algorithmic problem which sounds like this:

Given a three-dimensional space and segments in it. Find the point with minimal distance to all of the segments. Example input: in the first line N - the number of segments, in the N next lines given the begin and the end of each segment: x1 y1 z1 x2 y2 z2

I know what kind of a given problem it is (a geometrical median) and I already know how to find the minimal distance between point and segment (Cartesian distance and a nice code provided here), but what I need - is a point (x, y, z) on a segment to which I found the distance. I need to know it to approximate my result.

Here is the code I have

# finds distance between point and segment
def lineseg_dist(p, a, b):
    d = np.divide(b - a, np.linalg.norm(b - a))
    s = np.dot(a - p, d)
    t = np.dot(p - b, d)
    h = np.maximum.reduce([s, t, 0])
    c = np.cross(p - a, d)
    return np.hypot(h, np.linalg.norm(c))

#segment 
seg = [[1, 1, 1], [2, 2, 2]]
lineseg_dist([0, 0, 0], np.array(seg[0]), np.array(seg[1])) #1.73205

For example distance from [0, 0, 0] point to segment is known and we can say that the closest point of the segment to us is [1, 1, 1]. But how do we find the nearest point in other cases?

Upvotes: 0

Views: 1483

Answers (1)

aerobiomat
aerobiomat

Reputation: 3437

From your last paragraph I understand you need to find a point in a segment that is closest to another point.

This function returns both the closest point and the distance:

def closest_point_and_distance(p, a, b):
    s = b - a
    w = p - a
    ps = np.dot(w, s)
    if ps <= 0:
        return a, np.linalg.norm(w)
    l2 = np.dot(s, s)
    if ps >= l2:
        closest = b
    else:
        closest = a + ps / l2 * s
    return closest, np.linalg.norm(p - closest)

It is also faster than the code you have.

Upvotes: 4

Related Questions