I need a function to find the shortest distance between two line segments. A line segment is defined by two endpoints. So for example one of my line segments (AB) would be defined by the two points A (x1,y1) and B (x2,y2) and the other (CD) would be defined by the two points C (x1,y1) and D (x2,y2).
Feel free to write the solution in any language you want and I can translate it into javascript. Please keep in mind my geometry skills are pretty rusty. I have already seen here and I am not sure how to translate this into a function. Thank you so much for help.
This is my solution in Python. Works with 3d points and you can simplify for 2d.
import numpy as np
def closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False,clampA0=False,clampA1=False,clampB0=False,clampB1=False):
''' Given two lines defined by numpy.array pairs (a0,a1,b0,b1)
Return the closest points on each segment and their distance
'''
# If clampAll=True, set all clamps to True
if clampAll:
clampA0=True
clampA1=True
clampB0=True
clampB1=True
# Calculate denomitator
A = a1 - a0
B = b1 - b0
magA = np.linalg.norm(A)
magB = np.linalg.norm(B)
_A = A / magA
_B = B / magB
cross = np.cross(_A, _B);
denom = np.linalg.norm(cross)**2
# If lines are parallel (denom=0) test if lines overlap.
# If they don't overlap then there is a closest point solution.
# If they do overlap, there are infinite closest positions, but there is a closest distance
if not denom:
d0 = np.dot(_A,(b0-a0))
# Overlap only possible with clamping
if clampA0 or clampA1 or clampB0 or clampB1:
d1 = np.dot(_A,(b1-a0))
# Is segment B before A?
if d0 <= 0 >= d1:
if clampA0 and clampB1:
if np.absolute(d0) < np.absolute(d1):
return a0,b0,np.linalg.norm(a0-b0)
return a0,b1,np.linalg.norm(a0-b1)
# Is segment B after A?
elif d0 >= magA <= d1:
if clampA1 and clampB0:
if np.absolute(d0) < np.absolute(d1):
return a1,b0,np.linalg.norm(a1-b0)
return a1,b1,np.linalg.norm(a1-b1)
# Segments overlap, return distance between parallel segments
return None,None,np.linalg.norm(((d0*_A)+a0)-b0)
# Lines criss-cross: Calculate the projected closest points
t = (b0 - a0);
detA = np.linalg.det([t, _B, cross])
detB = np.linalg.det([t, _A, cross])
t0 = detA/denom;
t1 = detB/denom;
pA = a0 + (_A * t0) # Projected closest point on segment A
pB = b0 + (_B * t1) # Projected closest point on segment B
# Clamp projections
if clampA0 or clampA1 or clampB0 or clampB1:
if clampA0 and t0 < 0:
pA = a0
elif clampA1 and t0 > magA:
pA = a1
if clampB0 and t1 < 0:
pB = b0
elif clampB1 and t1 > magB:
pB = b1
# Clamp projection A
if (clampA0 and t0 < 0) or (clampA1 and t0 > magA):
dot = np.dot(_B,(pA-b0))
if clampB0 and dot < 0:
dot = 0
elif clampB1 and dot > magB:
dot = magB
pB = b0 + (_B * dot)
# Clamp projection B
if (clampB0 and t1 < 0) or (clampB1 and t1 > magB):
dot = np.dot(_A,(pB-a0))
if clampA0 and dot < 0:
dot = 0
elif clampA1 and dot > magA:
dot = magA
pA = a0 + (_A * dot)
return pA,pB,np.linalg.norm(pA-pB)
Test example with pictures to help visualize:
a1=np.array([13.43, 21.77, 46.81])
a0=np.array([27.83, 31.74, -26.60])
b0=np.array([77.54, 7.53, 6.22])
b1=np.array([26.99, 12.39, 11.18])
closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=True)
# Result: (array([ 20.29994362, 26.5264818 , 11.78759994]), array([ 26.99, 12.39, 11.18]), 15.651394495590445) #
closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False)
# Result: (array([ 19.85163563, 26.21609078, 14.07303667]), array([ 18.40058604, 13.21580716, 12.02279907]), 13.240709703623198) #
Taken from this example, which also comes with a simple explanation of why it works as well as VB code (that does more than you need, so I've simplified as I translated to Python -- note: I have translated, but not tested, so a typo might have slipped by...):
def segments_distance(x11, y11, x12, y12, x21, y21, x22, y22):
""" distance between two segments in the plane:
one segment is (x11, y11) to (x12, y12)
the other is (x21, y21) to (x22, y22)
"""
if segments_intersect(x11, y11, x12, y12, x21, y21, x22, y22): return 0
# try each of the 4 vertices w/the other segment
distances = []
distances.append(point_segment_distance(x11, y11, x21, y21, x22, y22))
distances.append(point_segment_distance(x12, y12, x21, y21, x22, y22))
distances.append(point_segment_distance(x21, y21, x11, y11, x12, y12))
distances.append(point_segment_distance(x22, y22, x11, y11, x12, y12))
return min(distances)
def segments_intersect(x11, y11, x12, y12, x21, y21, x22, y22):
""" whether two segments in the plane intersect:
one segment is (x11, y11) to (x12, y12)
the other is (x21, y21) to (x22, y22)
"""
dx1 = x12 - x11
dy1 = y12 - y11
dx2 = x22 - x21
dy2 = y22 - y21
delta = dx2 * dy1 - dy2 * dx1
if delta == 0: return False # parallel segments
s = (dx1 * (y21 - y11) + dy1 * (x11 - x21)) / delta
t = (dx2 * (y11 - y21) + dy2 * (x21 - x11)) / (-delta)
return (0 <= s <= 1) and (0 <= t <= 1)
import math
def point_segment_distance(px, py, x1, y1, x2, y2):
dx = x2 - x1
dy = y2 - y1
if dx == dy == 0: # the segment's just a point
return math.hypot(px - x1, py - y1)
# Calculate the t that minimizes the distance.
t = ((px - x1) * dx + (py - y1) * dy) / (dx * dx + dy * dy)
# See if this represents one of the segment's
# end points or a point in the middle.
if t < 0:
dx = px - x1
dy = py - y1
elif t > 1:
dx = px - x2
dy = py - y2
else:
near_x = x1 + t * dx
near_y = y1 + t * dy
dx = px - near_x
dy = py - near_y
return math.hypot(dx, dy)
Is this in 2 dimensions? If so, the answer is simply the shortest of the distance between point A and line segment CD, B and CD, C and AB or D and AB. So it's a fairly simple "distance between point and line" calculation (if the distances are all the same, then the lines are parallel).
This site explains the algorithm for distance between a point and a line pretty well.
It's slightly more tricky in the 3 dimensions because the lines are not necessarily in the same plane, but that doesn't seem to be the case here?
This is my solution. It's programmed in Lua. It is very concise, so maybe it will be appreciated. Please do make sure the lengths of the line segments are non 0.
local eta = 1e-6
local function nearestPointsOnLineSegments(a0, a1, b0, b1)
local r = b0 - a0
local u = a1 - a0
local v = b1 - b0
local ru = r:Dot(u)
local rv = r:Dot(v)
local uu = u:Dot(u)
local uv = u:Dot(v)
local vv = v:Dot(v)
local det = uu*vv - uv*uv
local s, t
if det < eta*uu*vv then
s = math.clamp(ru/uu, 0, 1)
t = 0
else
s = math.clamp((ru*vv - rv*uv)/det, 0, 1)
t = math.clamp((ru*uv - rv*uu)/det, 0, 1)
end
local S = math.clamp((t*uv + ru)/uu, 0, 1)
local T = math.clamp((s*uv - rv)/vv, 0, 1)
local A = a0 + S*u
local B = b0 + T*v
return A, B, (B - A):Length()
end
My solution is a translation of Fnord solution. I do in javascript and C.
In Javascript. You need to include mathjs.
var closestDistanceBetweenLines = function(a0, a1, b0, b1, clampAll, clampA0, clampA1, clampB0, clampB1){
//Given two lines defined by numpy.array pairs (a0,a1,b0,b1)
//Return distance, the two closest points, and their average
clampA0 = clampA0 || false;
clampA1 = clampA1 || false;
clampB0 = clampB0 || false;
clampB1 = clampB1 || false;
clampAll = clampAll || false;
if(clampAll){
clampA0 = true;
clampA1 = true;
clampB0 = true;
clampB1 = true;
}
//Calculate denomitator
var A = math.subtract(a1, a0);
var B = math.subtract(b1, b0);
var _A = math.divide(A, math.norm(A))
var _B = math.divide(B, math.norm(B))
var cross = math.cross(_A, _B);
var denom = math.pow(math.norm(cross), 2);
//If denominator is 0, lines are parallel: Calculate distance with a projection and evaluate clamp edge cases
if (denom == 0){
var d0 = math.dot(_A, math.subtract(b0, a0));
var d = math.norm(math.subtract(math.add(math.multiply(d0, _A), a0), b0));
//If clamping: the only time we'll get closest points will be when lines don't overlap at all. Find if segments overlap using dot products.
if(clampA0 || clampA1 || clampB0 || clampB1){
var d1 = math.dot(_A, math.subtract(b1, a0));
//Is segment B before A?
if(d0 <= 0 && 0 >= d1){
if(clampA0 == true && clampB1 == true){
if(math.absolute(d0) < math.absolute(d1)){
return [b0, a0, math.norm(math.subtract(b0, a0))];
}
return [b1, a0, math.norm(math.subtract(b1, a0))];
}
}
//Is segment B after A?
else if(d0 >= math.norm(A) && math.norm(A) <= d1){
if(clampA1 == true && clampB0 == true){
if(math.absolute(d0) < math.absolute(d1)){
return [b0, a1, math.norm(math.subtract(b0, a1))];
}
return [b1, a1, math.norm(math.subtract(b1,a1))];
}
}
}
//If clamping is off, or segments overlapped, we have infinite results, just return position.
return [null, null, d];
}
//Lines criss-cross: Calculate the dereminent and return points
var t = math.subtract(b0, a0);
var det0 = math.det([t, _B, cross]);
var det1 = math.det([t, _A, cross]);
var t0 = math.divide(det0, denom);
var t1 = math.divide(det1, denom);
var pA = math.add(a0, math.multiply(_A, t0));
var pB = math.add(b0, math.multiply(_B, t1));
//Clamp results to line segments if needed
if(clampA0 || clampA1 || clampB0 || clampB1){
if(t0 < 0 && clampA0)
pA = a0;
else if(t0 > math.norm(A) && clampA1)
pA = a1;
if(t1 < 0 && clampB0)
pB = b0;
else if(t1 > math.norm(B) && clampB1)
pB = b1;
}
var d = math.norm(math.subtract(pA, pB))
return [pA, pB, d];
}
//example
var a1=[13.43, 21.77, 46.81];
var a0=[27.83, 31.74, -26.60];
var b0=[77.54, 7.53, 6.22];
var b1=[26.99, 12.39, 11.18];
closestDistanceBetweenLines(a0,a1,b0,b1,true);
In pure C
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
double determinante3(double* a, double* v1, double* v2){
return a[0] * (v1[1] * v2[2] - v1[2] * v2[1]) + a[1] * (v1[2] * v2[0] - v1[0] * v2[2]) + a[2] * (v1[0] * v2[1] - v1[1] * v2[0]);
}
double* cross3(double* v1, double* v2){
double* v = (double*)malloc(3 * sizeof(double));
v[0] = v1[1] * v2[2] - v1[2] * v2[1];
v[1] = v1[2] * v2[0] - v1[0] * v2[2];
v[2] = v1[0] * v2[1] - v1[1] * v2[0];
return v;
}
double dot3(double* v1, double* v2){
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
double norma3(double* v1){
double soma = 0;
for (int i = 0; i < 3; i++) {
soma += pow(v1[i], 2);
}
return sqrt(soma);
}
double* multiplica3(double* v1, double v){
double* v2 = (double*)malloc(3 * sizeof(double));
for (int i = 0; i < 3; i++) {
v2[i] = v1[i] * v;
}
return v2;
}
double* soma3(double* v1, double* v2, int sinal){
double* v = (double*)malloc(3 * sizeof(double));
for (int i = 0; i < 3; i++) {
v[i] = v1[i] + sinal * v2[i];
}
return v;
}
Result_distance* closestDistanceBetweenLines(double* a0, double* a1, double* b0, double* b1, int clampAll, int clampA0, int clampA1, int clampB0, int clampB1){
double denom, det0, det1, t0, t1, d;
double *A, *B, *_A, *_B, *cross, *t, *pA, *pB;
Result_distance *rd = (Result_distance *)malloc(sizeof(Result_distance));
if (clampAll){
clampA0 = 1;
clampA1 = 1;
clampB0 = 1;
clampB1 = 1;
}
A = soma3(a1, a0, -1);
B = soma3(b1, b0, -1);
_A = multiplica3(A, 1 / norma3(A));
_B = multiplica3(B, 1 / norma3(B));
cross = cross3(_A, _B);
denom = pow(norma3(cross), 2);
if (denom == 0){
double d0 = dot3(_A, soma3(b0, a0, -1));
d = norma3(soma3(soma3(multiplica3(_A, d0), a0, 1), b0, -1));
if (clampA0 || clampA1 || clampB0 || clampB1){
double d1 = dot3(_A, soma3(b1, a0, -1));
if (d0 <= 0 && 0 >= d1){
if (clampA0 && clampB1){
if (abs(d0) < abs(d1)){
rd->pA = b0;
rd->pB = a0;
rd->d = norma3(soma3(b0, a0, -1));
}
else{
rd->pA = b1;
rd->pB = a0;
rd->d = norma3(soma3(b1, a0, -1));
}
}
}
else if (d0 >= norma3(A) && norma3(A) <= d1){
if (clampA1 && clampB0){
if (abs(d0) <abs(d1)){
rd->pA = b0;
rd->pB = a1;
rd->d = norma3(soma3(b0, a1, -1));
}
else{
rd->pA = b1;
rd->pB = a1;
rd->d = norma3(soma3(b1, a1, -1));
}
}
}
}
else{
rd->pA = NULL;
rd->pB = NULL;
rd->d = d;
}
}
else{
t = soma3(b0, a0, -1);
det0 = determinante3(t, _B, cross);
det1 = determinante3(t, _A, cross);
t0 = det0 / denom;
t1 = det1 / denom;
pA = soma3(a0, multiplica3(_A, t0), 1);
pB = soma3(b0, multiplica3(_B, t1), 1);
if (clampA0 || clampA1 || clampB0 || clampB1){
if (t0 < 0 && clampA0)
pA = a0;
else if (t0 > norma3(A) && clampA1)
pA = a1;
if (t1 < 0 && clampB0)
pB = b0;
else if (t1 > norma3(B) && clampB1)
pB = b1;
}
d = norma3(soma3(pA, pB, -1));
rd->pA = pA;
rd->pB = pB;
rd->d = d;
}
free(A);
free(B);
free(cross);
free(t);
return rd;
}
int main(void){
//example
double a1[] = { 13.43, 21.77, 46.81 };
double a0[] = { 27.83, 31.74, -26.60 };
double b0[] = { 77.54, 7.53, 6.22 };
double b1[] = { 26.99, 12.39, 11.18 };
Result_distance* rd = closestDistanceBetweenLines(a0, a1, b0, b1, 1, 0, 0, 0, 0);
printf("pA = [%f, %f, %f]\n", rd->pA[0], rd->pA[1], rd->pA[2]);
printf("pB = [%f, %f, %f]\n", rd->pB[0], rd->pB[1], rd->pB[2]);
printf("d = %f\n", rd->d);
return 0;
}
For calculating the minimum distance between 2 2D line segments it is true that you have to perform 4 perpendicular distance from endpoint to other line checks successively using each of the 4 endpoints. However, if you find that the perpendicular line drawn out does not intersect the line segment in any of the 4 cases then you have to perform 4 additional endpoint to endpoint distance checks to find the shortest distance.
Whether there is a more elegent solution to this I do not know.
Please note that the above solutions are correct under the assumption that the line segments do not intersect! If the line segments intersect it is clear that their distance should be 0. It is therefore necessary to one final check which is: Suppose the distance between point A and CD, d(A,CD), was the smallest of the 4 checks mentioned by Dean. Then take a small step along the segment AB from point A. Denote this point E. If d(E,CD) < d(A,CD), the segments must be intersecting! Note that this will never be the case addressed by Stephen.
This is the basic code I follow for the shortest distance between any two plan or any two points in the 3d plane it works well metrics can be changed for the given input
Code In PYTHON
def dot(c1,c2):
return c1[0]* c2[0] + c1[1] * c2[1] + c1[2] * c2[2]
def norm(c1):
return math.sqrt(dot(c1, c1))
def getShortestDistance(x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4):
print(x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4)
EPS = 0.00000001
delta21 = [1,2,3]
delta21[0] = x2 - x1
delta21[1] = y2 - y1
delta21[2] = z2 - z1
delta41 = [1,2,3]
delta41[0] = x4 - x3
delta41[1] = y4 - y3
delta41[2] = z4 - z3
delta13 = [1,2,3]
delta13[0] = x1 - x3
delta13[1] = y1 - y3
delta13[2] = z1 - z3
a = dot(delta21, delta21)
b = dot(delta21, delta41)
c = dot(delta41, delta41)
d = dot(delta21, delta13)
e = dot(delta41, delta13)
D = a * c - b * b
sc = D
sN = D
sD = D
tc = D
tN = D
tD = D
if D < EPS:
sN = 0.0
sD = 1.0
tN = e
tD = c
else:
sN = (b * e - c * d)
tN = (a * e - b * d)
if sN < 0.0:
sN = 0.0
tN = e
tD = c
elif sN > sD:
sN = sD
tN = e + b
tD = c
if tN < 0.0:
tN = 0.0
if -d < 0.0:
sN = 0.0
elif -d > a:
sN = sD
else:
sN = -d
sD = a
elif tN > tD:
tN = tD
if ((-d + b) < 0.0):
sN = 0
elif ((-d + b) > a):
sN = sD
else:
sN = (-d + b)
sD = a
if (abs(sN) < EPS):
sc = 0.0
else:
sc = sN / sD
if (abs(tN) < EPS):
tc = 0.0
else:
tc = tN / tD
dP = [1,2,3]
dP[0] = delta13[0] + (sc * delta21[0]) - (tc * delta41[0])
dP[1] = delta13[1] + (sc * delta21[1]) - (tc * delta41[1])
dP[2] = delta13[2] + (sc * delta21[2]) - (tc * delta41[2])
return math.sqrt(dot(dP, dP))
This solution is in essence the one from Alex Martelli, but I've added a Point and a LineSegment class to make reading easier. I also adjusted the formatting and added some tests.
The line segment intersection is wrong, but it seems not to matter for the calculation of the distance of line segments. If you're interested in a correct line segment intersection thest, look here: How do you detect whether or not two line segments intersect?
#!/usr/bin/env python
"""Calculate the distance between line segments."""
import math
class Point(object):
"""A two dimensional point."""
def __init__(self, x, y):
self.x = float(x)
self.y = float(y)
class LineSegment(object):
"""A line segment in a two dimensional space."""
def __init__(self, p1, p2):
assert isinstance(p1, Point), \
"p1 is not of type Point, but of %r" % type(p1)
assert isinstance(p2, Point), \
"p2 is not of type Point, but of %r" % type(p2)
self.p1 = p1
self.p2 = p2
def segments_distance(segment1, segment2):
"""Calculate the distance between two line segments in the plane.
>>> a = LineSegment(Point(1,0), Point(2,0))
>>> b = LineSegment(Point(0,1), Point(0,2))
>>> "%0.2f" % segments_distance(a, b)
'1.41'
>>> c = LineSegment(Point(0,0), Point(5,5))
>>> d = LineSegment(Point(2,2), Point(4,4))
>>> e = LineSegment(Point(2,2), Point(7,7))
>>> "%0.2f" % segments_distance(c, d)
'0.00'
>>> "%0.2f" % segments_distance(c, e)
'0.00'
"""
if segments_intersect(segment1, segment2):
return 0
# try each of the 4 vertices w/the other segment
distances = []
distances.append(point_segment_distance(segment1.p1, segment2))
distances.append(point_segment_distance(segment1.p2, segment2))
distances.append(point_segment_distance(segment2.p1, segment1))
distances.append(point_segment_distance(segment2.p2, segment1))
return min(distances)
def segments_intersect(segment1, segment2):
"""Check if two line segments in the plane intersect.
>>> segments_intersect(LineSegment(Point(0,0), Point(1,0)), \
LineSegment(Point(0,0), Point(1,0)))
True
"""
dx1 = segment1.p2.x - segment1.p1.x
dy1 = segment1.p2.y - segment1.p2.y
dx2 = segment2.p2.x - segment2.p1.x
dy2 = segment2.p2.y - segment2.p1.y
delta = dx2 * dy1 - dy2 * dx1
if delta == 0: # parallel segments
# TODO: Could be (partially) identical!
return False
s = (dx1 * (segment2.p1.y - segment1.p1.y) +
dy1 * (segment1.p1.x - segment2.p1.x)) / delta
t = (dx2 * (segment1.p1.y - segment2.p1.y) +
dy2 * (segment2.p1.x - segment1.p1.x)) / (-delta)
return (0 <= s <= 1) and (0 <= t <= 1)
def point_segment_distance(point, segment):
"""
>>> a = LineSegment(Point(1,0), Point(2,0))
>>> b = LineSegment(Point(2,0), Point(0,2))
>>> point_segment_distance(Point(0,0), a)
1.0
>>> "%0.2f" % point_segment_distance(Point(0,0), b)
'1.41'
"""
assert isinstance(point, Point), \
"point is not of type Point, but of %r" % type(point)
dx = segment.p2.x - segment.p1.x
dy = segment.p2.y - segment.p1.y
if dx == dy == 0: # the segment's just a point
return math.hypot(point.x - segment.p1.x, point.y - segment.p1.y)
if dx == 0:
if (point.y <= segment.p1.y or point.y <= segment.p2.y) and \
(point.y >= segment.p2.y or point.y >= segment.p2.y):
return abs(point.x - segment.p1.x)
if dy == 0:
if (point.x <= segment.p1.x or point.x <= segment.p2.x) and \
(point.x >= segment.p2.x or point.x >= segment.p2.x):
return abs(point.y - segment.p1.y)
# Calculate the t that minimizes the distance.
t = ((point.x - segment.p1.x) * dx + (point.y - segment.p1.y) * dy) / \
(dx * dx + dy * dy)
# See if this represents one of the segment's
# end points or a point in the middle.
if t < 0:
dx = point.x - segment.p1.x
dy = point.y - segment.p1.y
elif t > 1:
dx = point.x - segment.p2.x
dy = point.y - segment.p2.y
else:
near_x = segment.p1.x + t * dx
near_y = segment.p1.y + t * dy
dx = point.x - near_x
dy = point.y - near_y
return math.hypot(dx, dy)
if __name__ == '__main__':
import doctest
doctest.testmod()
I have made a Swift port based on Pratik Deoghare's answer above. Pratik references Dan Sunday's excellent write-up and code examples found here: http://geomalgorithms.com/a07-_distance.html
The following functions calculate the minimum distance between two lines or two line segments, and is a direct port of Dan Sunday's C++ examples.
The LASwift linear algebra package is used to do the matrix and vector calculations.
//
// This is a Swift port of the C++ code here
// http://geomalgorithms.com/a07-_distance.html
//
// Copyright 2001 softSurfer, 2012 Dan Sunday
// This code may be freely used, distributed and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.
//
//
// LASwift is a "Linear Algebra library for Swift language" by Alexander Taraymovich
// https://github.com/AlexanderTar/LASwift
// LASwift is available under the BSD-3-Clause license.
//
// I've modified the lineToLineDistance and segmentToSegmentDistance functions
// to also return the points on each line/segment where the distance is shortest.
//
import LASwift
import Foundation
func norm(_ v: Vector) -> Double {
return sqrt(dot(v,v)) // norm = length of vector
}
func d(_ u: Vector, _ v: Vector) -> Double {
return norm(u-v) // distance = norm of difference
}
let SMALL_NUM = 0.000000000000000001 // anything that avoids division overflow
typealias Point = Vector
struct Line {
let P0: Point
let P1: Point
}
struct Segment {
let P0: Point
let P1: Point
}
// lineToLineDistance(): get the 3D minimum distance between 2 lines
// Input: two 3D lines L1 and L2
// Return: the shortest distance between L1 and L2
func lineToLineDistance(L1: Line, L2: Line) -> (P1: Point, P2: Point, D: Double) {
let u = L1.P1 - L1.P0
let v = L2.P1 - L2.P0
let w = L1.P0 - L2.P0
let a = dot(u,u) // always >= 0
let b = dot(u,v)
let c = dot(v,v) // always >= 0
let d = dot(u,w)
let e = dot(v,w)
let D = a*c - b*b // always >= 0
var sc, tc: Double
// compute the line parameters of the two closest points
if D < SMALL_NUM { // the lines are almost parallel
sc = 0.0
tc = b>c ? d/b : e/c // use the largest denominator
}
else {
sc = (b*e - c*d) / D
tc = (a*e - b*d) / D
}
// get the difference of the two closest points
let dP = w + (sc .* u) - (tc .* v) // = L1(sc) - L2(tc)
let Psc = L1.P0 + sc .* u
let Qtc = L2.P0 + tc .* v
let dP2 = Psc - Qtc
assert(dP == dP2)
return (P1: Psc, P2: Qtc, D: norm(dP)) // return the closest distance
}
// segmentToSegmentDistance(): get the 3D minimum distance between 2 segments
// Input: two 3D line segments S1 and S2
// Return: the shortest distance between S1 and S2
func segmentToSegmentDistance(S1: Segment, S2: Segment) -> (P1: Point, P2: Point, D: Double) {
let u = S1.P1 - S1.P0
let v = S2.P1 - S2.P0
let w = S1.P0 - S2.P0
let a = dot(u,u) // always >= 0
let b = dot(u,v)
let c = dot(v,v) // always >= 0
let d = dot(u,w)
let e = dot(v,w)
let D = a*c - b*b // always >= 0
let sc: Double
var sN: Double
var sD = D // sc = sN / sD, default sD = D >= 0
let tc: Double
var tN: Double
var tD = D // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < SMALL_NUM) { // the lines are almost parallel
sN = 0.0 // force using point P0 on segment S1
sD = 1.0 // to prevent possible division by 0.0 later
tN = e
tD = c
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d)
tN = (a*e - b*d)
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0
tN = e
tD = c
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD
tN = e + b
tD = c
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0
// recompute sc for this edge
if (-d < 0.0) {
sN = 0.0
}
else if (-d > a) {
sN = sD
}
else {
sN = -d
sD = a
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0) {
sN = 0
}
else if ((-d + b) > a) {
sN = sD
}
else {
sN = (-d + b)
sD = a
}
}
// finally do the division to get sc and tc
sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD)
tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD)
// get the difference of the two closest points
let dP = w + (sc .* u) - (tc .* v) // = S1(sc) - S2(tc)
let Psc = S1.P0 + sc .* u
let Qtc = S2.P0 + tc .* v
let dP2 = Psc - Qtc
assert(dP == dP2)
return (P1: Psc, P2: Qtc, D: norm(dP)) // return the closest distance
}
Here's a Java solution (done the easy way with point checking, so probably not as efficient):
public static double getDistanceBetweenLineSegments(
PointDouble line1Start, PointDouble line1End,
PointDouble line2Start, PointDouble line2End) {
double result = 0;
// If they don't intersect, then work out the distance
if (!isLineIntersectingLine(line1Start, line1End, line2Start, line2End)) {
double p1 = getDistanceBetweenPointAndLine(line1Start, line2Start, line2End);
double p2 = getDistanceBetweenPointAndLine(line1End, line2Start, line2End);
double p3 = getDistanceBetweenPointAndLine(line2Start, line1Start, line1End);
double p4 = getDistanceBetweenPointAndLine(line2End, line1Start, line1End);
result = MathSafe.min(p1, MathSafe.min(p2, MathSafe.min(p3, p4)));
}
return result;
}
And all the other code you need:
public class PointDouble {
private double x;
private double y;
public PointDouble(double x, double y) {
this.x = x;
this.y = y;
}
public double getX() {
return x;
}
public double getY() {
return y;
}
}
private static int relativeCCW(
double x1, double y1,
double x2, double y2,
double px, double py) {
x2 -= x1;
y2 -= y1;
px -= x1;
py -= y1;
double ccw = px * y2 - py * x2;
if (ccw == 0.0) {
ccw = px * x2 + py * y2;
if (ccw > 0.0) {
px -= x2;
py -= y2;
ccw = px * x2 + py * y2;
if (ccw < 0.0) {
ccw = 0.0;
}
}
}
return (ccw < 0.0) ? -1 : ((ccw > 0.0) ? 1 : 0);
}
public static boolean isLineIntersectingLine(
PointDouble line1Start, PointDouble line1End,
PointDouble line2Start, PointDouble line2End) {
return (
(relativeCCW(line1Start.getX(), line1Start.getY(), line1End.getX(), line1End.getY(), line2Start.getX(), line2Start.getY()) *
relativeCCW(line1Start.getX(), line1Start.getY(), line1End.getX(), line1End.getY(), line2End.getX(), line2End.getY()) <= 0)
&&
(relativeCCW(line2Start.getX(), line2Start.getY(), line2End.getX(), line2End.getY(), line1Start.getX(), line1Start.getY()) *
relativeCCW(line2Start.getX(), line2Start.getY(), line2End.getX(), line2End.getY(), line1End.getX(), line1End.getY()) <= 0));
}
public static double getDistanceBetweenPointAndLine(PointDouble pt, PointDouble linePt1, PointDouble linePt2) {
double lineX = linePt2.getX() - linePt1.getX();
double lineY = linePt2.getY() - linePt1.getY();
double dot = (pt.getX() - linePt1.getX()) * lineX + (pt.getY() - linePt1.getY()) * lineY;
double len_sq = lineX * lineX + lineY * lineY;
double param = -1;
double xx;
double yy;
if (len_sq != 0) {
param = dot / len_sq;
}
if (param < 0) {
xx = linePt1.getX();
yy = linePt1.getY();
}
else if (param > 1) {
xx = linePt2.getX();
yy = linePt2.getY();
}
else {
xx = linePt1.getX() + param * lineX;
yy = linePt1.getY() + param * lineY;
}
return MathSafe.hypot(pt.getX() - xx, pt.getY() - yy);
}
Here is one in perl with a few differences from Fnord's:
It always clamps (add non-clamping behavior flags, if you wish, by following Fnord's example).
It catches lines of zero-length line segments that would otherwise cause a divide by zero.
The 1e-6 values below are used to deal with possible floating point errors. Adjust or remove those if you need a different tolerance:
use strict;
use warnings;
use Math::Vector::Real;
use Math::Matrix;
# True if $a >= $b within $tolerance
sub approx_greater
{
my ($a, $b, $tolerance) = #_;
$tolerance //= 1e-6;
return ($a-$b) > -$tolerance;
}
# True if $a <= $b within $tolerance
sub approx_lesser
{
my ($a, $b, $tolerance) = #_;
$tolerance //= 1e-6;
return ($b-$a) > -$tolerance;
}
# True if $a == $b within $tolerance
sub approx_equal
{
my ($a, $b, $tolerance) = #_;
$tolerance //= 1e-6;
return abs($a-$b) < $tolerance;
}
# Returns shortest line: [ [x1,y1,z1], [x2,y2,z2], distance ].
# If askew lines cross (or nearly-intersect) then xyz1 and xyz2 are undefined
# and only distance is returned.
#
# Thank you to #Fnord: https://stackoverflow.com/a/18994296/14055985
sub line_intersect
{
# Map #_ as vectors:
my ($a0, $a1, $b0, $b1) = map { V(#{ $_ }) } #_;
my $A = ($a1-$a0);
my $B = ($b1-$b0);
my $magA = abs($A);
my $magB = abs($B);
# If length line segment:
if ($magA == 0 || $magB == 0)
{
return V(undef, undef, 0);
}
my $_A = $A / $magA;
my $_B = $B / $magB;
my $cross = $_A x $_B;
my $denom = $cross->norm2;
# If lines are parallel (denom=0) test if lines overlap.
# If they don't overlap then there is a closest point solution.
# If they do overlap, there are infinite closest positions, but there is a closest distance
#if ($denom == 0)
if (approx_equal($denom, 0))
{
my $d0 = $_A * ($b0-$a0);
my $d1 = $_A * ($b1-$a0);
# Is segment B before A?
#if ($d0 <= 0 && 0 >= $d1)
if (approx_lesser($d0, 0) && approx_greater(0, $d1))
{
if (abs($d0) < abs($d1))
{
return V($a0, $b0, abs($a0-$b0));
}
else
{
return V($a0, $b1, abs($a0-$b1));
}
}
# Is segment B after A?
#elsif ($d0 >= $magA && $magA <= $d1)
elsif (approx_greater($d0, $magA) && approx_lesser($magA, $d1))
{
if (abs($d0) < abs($d1))
{
return V($a1, $b0, abs($a1-$b0));
}
else
{
return V($a1, $b1, abs($a1-$b1));
}
}
else
{
# Segments overlap, return distance between parallel segments
return V(V(), V(), abs((($d0*$_A)+$a0)-$b0));
}
}
else
{
# Lines criss-cross: Calculate the projected closest points
my $t = ($b0 - $a0);
# Math::Matrix won't wirth with Math::Vector::Real
# even though they are blessed arrays,
# so convert them to arrays and back to refs:
my $detA = Math::Matrix->new([ [ #$t ], [ #$_B ], [ #$cross] ])->det;
my $detB = Math::Matrix->new([ [ #$t ], [ #$_A ], [ #$cross] ])->det;
my $t0 = $detA / $denom;
my $t1 = $detB / $denom;
my $pA = $a0 + ($_A * $t0); # Projected closest point on segment A
my $pB = $b0 + ($_B * $t1); # Projected closest point on segment A
if ($t0 < 0)
{
$pA = $a0;
}
elsif ($t0 > $magA)
{
$pA = $a1;
}
if ($t1 < 0)
{
$pB = $b0;
}
elsif ($t1 > $magB)
{
$pB = $b1;
}
# Clamp projection A
if ($t0 < 0 || $t0 > $magA)
{
my $dot = $_B * ($pA-$b0);
if ($dot < 0)
{
$dot = 0;
}
elsif ($dot > $magB)
{
$dot = $magB;
}
$pB = $b0 + ($_B * $dot)
}
# Clamp projection B
if ($t1 < 0 || $t1 > $magB)
{
my $dot = $_A * ($pB-$a0);
if ($dot < 0)
{
$dot = 0;
}
elsif ($dot > $magA)
{
$dot = $magA;
}
$pA = $a0 + ($_A * $dot)
}
return V($pA, $pB, abs($pA-$pB));
}
}
print "example: " . line_intersect(
[13.43, 21.77, 46.81 ], [27.83, 31.74, -26.60 ],
[77.54, 7.53, 6.22 ], [26.99, 12.39, 11.18 ]) . "\n" ;
print "contiguous: " . line_intersect(
[0, 0, 0 ], [ 0, 0, 1 ],
[0, 0, 1 ], [ 0, 0, 2 ],
) . "\n" ;
print "contiguous 90: " . line_intersect(
[0, 0, 0 ], [ 0, 0, 1 ],
[0, 0, 1 ], [ 0, 1, 1 ],
) . "\n" ;
print "colinear separate: " . line_intersect(
[0, 0, 0 ], [ 0, 0, 1 ],
[0, 0, 2 ], [ 0, 0, 3 ],
) . "\n" ;
print "cross: " . line_intersect(
[1, 1, 0 ], [ -1, -1, 0 ],
[-1, 1, 0 ], [ 1, -1, 0 ],
) . "\n" ;
print "cross+z: " . line_intersect(
[1, 1, 0 ], [ -1, -1, 0 ],
[-1, 1, 1 ], [ 1, -1, 1 ],
) . "\n" ;
print "full overlap1: " . line_intersect(
[2, 0, 0 ], [ 5, 0, 0 ],
[3, 0, 0 ], [ 4, 0, 0 ],
) . "\n" ;
print "full overlap2: " . line_intersect(
[3, 0, 0 ], [ 4, 0, 0 ],
[2, 0, 0 ], [ 5, 0, 0 ],
) . "\n" ;
print "partial overlap1: " . line_intersect(
[2, 0, 0 ], [ 5, 0, 0 ],
[3, 0, 0 ], [ 6, 0, 0 ],
) . "\n" ;
print "partial overlap2: " . line_intersect(
[3, 0, 0 ], [ 6, 0, 0 ],
[2, 0, 0 ], [ 5, 0, 0 ],
) . "\n" ;
print "parallel: " . line_intersect(
[3, 0, 0 ], [ 6, 0, 0 ],
[3, 0, 1 ], [ 6, 0, 1 ],
) . "\n" ;
Output
example: {{20.29994361624, 26.5264817954106, 11.7875999397098}, {26.99, 12.39, 11.18}, 15.6513944955904}
contiguous: {{0, 0, 1}, {0, 0, 1}, 0}
contiguous 90: {{0, 0, 1}, {0, 0, 1}, 0}
colinear separate: {{0, 0, 1}, {0, 0, 2}, 1}
cross: {{-2.22044604925031e-16, -2.22044604925031e-16, 0}, {2.22044604925031e-16, -2.22044604925031e-16, 0}, 4.44089209850063e-16}
cross+z: {{-2.22044604925031e-16, -2.22044604925031e-16, 0}, {2.22044604925031e-16, -2.22044604925031e-16, 1}, 1}
full overlap1: {{}, {}, 0}
full overlap2: {{}, {}, 0}
partial overlap1: {{}, {}, 0}
partial overlap2: {{}, {}, 0}
parallel: {{}, {}, 1}
Here is the solution from Fnord just for Ray-Ray Intersection in c# (infinite Lines, not Line segments)
It requires System.Numerics.Vector3
public static (Vector3 pointRayA, Vector3 pointRayB) ClosestPointRayRay((Vector3 point, Vector3 dir) rayA,
(Vector3 point, Vector3 dir) rayB)
{
var a = Normalize(rayA.dir);
var b = Normalize(rayB.dir);
var cross = Vector3.Cross(a, b);
var crossMag = cross.Length();
var denominator = crossMag * crossMag;
var t = rayB.point - rayA.point;
var detA = new Matrix3X3(t, b, cross).Det;
var detB = new Matrix3X3(t, a, cross).Det;
var t0 = detA / denominator;
var t1 = detB / denominator;
var pa = rayA.point + (a * t0);
var pb = rayB.point + (b * t1);
return (pa, pb);
}
public struct Matrix3X3
{
private float a11, a12, a13, a21, a22, a23, a31, a32, a33;
public Matrix3X3(Vector3 col1, Vector3 col2, Vector3 col3)
{
a11 = col1.X;
a21 = col1.Y;
a31 = col1.Z;
a12 = col2.X;
a22 = col2.Y;
a32 = col2.Z;
a13 = col3.X;
a23 = col3.Y;
a33 = col3.Z;
}
public float Det =>
a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32 -
(a31 * a22 * a13 + a32 * a23 * a11 + a33 * a21 * a12);
}
it returns the closest point on RayA to RayB ,named pointRayA, and vice versa.
A short test:
[Test]
public void RayRayIntersection()
{
var lineABase = new Vector3(0, 0, 0);
var lineADir = new Vector3(0, 0, 1);
var lineBBase = new Vector3(1, 0, 0);
var lineBDir = new Vector3(0, 1, 0);
var res = ClosestPointRayRay((lineABase, lineADir), (lineBBase, lineBDir));
Console.WriteLine(res);
}
returns (<0, 0, 0>, <1, 0, 0>)
I was looking for a way to compute the shortest distance between a large number of 3D lines. The code from Fnord was a start, but it was for only a single batch. I ported most of it to multi-batch, below.
import numpy as np
import time
def closest_distance_between_lines(a0, a1, b0, b1):
# equation for a line
number_of_lines = a0.shape[0]
A = a1 - a0
B = b1 - b0
# get the magnitude of each line
magA = np.broadcast_to(np.expand_dims(np.linalg.norm(A, axis=1), axis=1), (number_of_lines, 3))
magB = np.broadcast_to(np.expand_dims(np.linalg.norm(B, axis=1), axis=1), (number_of_lines, 3))
# get the unit-normalized lines
_A = A / magA
_B = B / magB
# get the perpendicular line
cross = np.cross(_A, _B, axis=1)
# normalize the perpendicular line
denom = np.linalg.norm(cross, axis=1)**2
# get the line between the start of the previous two lines
t = (b0 - a0)
stacked_matrices_A = np.stack([t, _B, cross], axis=1)
detA = np.linalg.det(stacked_matrices_A)
stacked_matrices_B = np.stack([t, _A, cross], axis=1)
detB = np.linalg.det(stacked_matrices_B)
t0 = detA/denom
t1 = detB/denom
t0 = np.broadcast_to(np.expand_dims(t0, axis=1), (number_of_lines, 3))
t1 = np.broadcast_to(np.expand_dims(t1, axis=1), (number_of_lines, 3))
# get the points that represent the closest projected distance between the two lines
pA = a0 + (_A * t0)
pB = b0 + (_B * t1)
return pA, pB, np.linalg.norm(pA-pB, axis=1)
number_of_samples = 10000000
a0 = np.random.rand(number_of_samples,3)
a1 = np.random.rand(number_of_samples,3)
b0 = np.random.rand(number_of_samples,3)
b1 = np.random.rand(number_of_samples,3)
start_time = time.time()
point_line_a, point_line_b, distance = closest_distance_between_lines(a0, a1, b0, b1)
end_time = time.time()
print("\nPoints pA on line A that are closest to line B with shape {}:\n{}".format(point_line_a.shape, point_line_a))
print("\nPoints pB on line B that are closest to line A with shape {}:\n{}".format(point_line_b.shape, point_line_b))
print("\nDistances between pA and pB with shape {}:\n{}".format(distance.shape, distance))
print("\n{:.2} seconds to compute closest distances between {:,} lines\n".format(end_time - start_time, number_of_samples))
Here is the output of above:
Points pA on line A that are closest to line B with shape (10000000, 3):
[[0.51075268 0.63261352 0.41417815]
[0.88225225 0.41163515 0.06090485]
[0.27712801 0.99045177 0.58932854]
...
[0.25982217 0.11225041 0.79015618]
[0.58653313 0.47864821 0.11680724]
[0.38297058 0.2251661 1.11088736]]
Points pB on line B that are closest to line A with shape (10000000, 3):
[[0.5084189 0.42417817 0.5847618 ]
[0.83041058 0.38914519 0.02384158]
[0.26068716 0.98567827 0.5010647 ]
...
[0.34356827 0.42162445 0.75820875]
[0.44523571 0.40278146 0.0014156 ]
[0.28498604 0.23670301 1.00712087]]
Distances between pA and pB with shape (10000000,):
[0.26935018 0.0675799 0.08990881 ... 0.32209679 0.19757518 0.14318364]
8.3 seconds to compute closest distances between 10,000,000 lines
Note, the code does not check for parallel lines, and it technically considers the entire line (rather than just the line segment).
To make it even faster, below is the same code ported to PyTorch:
import torch
import time
device = torch.device('cuda:0')
def closest_distance_between_lines(a0, a1, b0, b1):
number_of_lines = a0.shape[0]
# equation for a line
A = a1 - a0
B = b1 - b0
# get the magnitude of each line
magA = torch.broadcast_to(torch.unsqueeze(torch.linalg.norm(A, dim=1), dim=1), (number_of_lines, 3))
magB = torch.broadcast_to(torch.unsqueeze(torch.linalg.norm(B, dim=1), dim=1), (number_of_lines, 3))
# get the unit-normalized lines
_A = A / magA
_B = B / magB
# get the perpendicular line
cross = torch.cross(_A, _B, dim=1)
# normalize the perpendicular line
denom = torch.linalg.norm(cross, dim=1)**2
# get the line between the start of the previous two lines
t = (b0 - a0)
stacked_matrices_A = torch.stack([t, _B, cross], dim=1)
detA = torch.linalg.det(stacked_matrices_A)
stacked_matrices_B = torch.stack([t, _A, cross], dim=1)
detB = torch.linalg.det(stacked_matrices_B)
t0 = detA / denom
t1 = detB / denom
t0 = torch.broadcast_to(torch.unsqueeze(t0, dim=1), (number_of_lines, 3))
t1 = torch.broadcast_to(torch.unsqueeze(t1, dim=1), (number_of_lines, 3))
# get the points that represent the closest projected distance between the two lines
pA = a0 + (_A * t0)
pB = b0 + (_B * t1)
return pA, pB, torch.linalg.norm(pA-pB, dim=1)
number_of_samples = 10000000
a0 = torch.rand(number_of_samples,3).to(device=device)
a1 = torch.rand(number_of_samples,3).to(device=device)
b0 = torch.rand(number_of_samples,3).to(device=device)
b1 = torch.rand(number_of_samples,3).to(device=device)
start_time = time.time()
point_line_a, point_line_b, distance = closest_distance_between_lines(a0, a1, b0, b1)
end_time = time.time()
print("\nPoints pA on line A that are closest to line B with shape {}:\n{}".format(point_line_a.shape, point_line_a))
print("\nPoints pB on line B that are closest to line A with shape {}:\n{}".format(point_line_b.shape, point_line_b))
print("\nDistances between pA and pB with shape {}:\n{}".format(distance.shape, distance))
print("\n{:.2} seconds to compute closest distances between {:,} lines\n".format(end_time - start_time, number_of_samples))
On my GPU, the runtime from NumPy to Torch speeds up from 8.3 seconds to 0.13 seconds.
Enjoy.