我需要一个函数来找到两条线段之间的最短距离。一条线段由两个端点定义。因此，例如，我的一条线段 (AB) 将由两个点 A (x1,y1) 和 B (x2,y2) 定义，另一个 (CD) 将由两个点 C (x1,y1) 定义和 D (x2,y2)。
请随意用您想要的任何语言编写解决方案，我可以将其翻译成 javascript。请记住，我的几何技能很生疏。我已经看过here，但我不确定如何将其转换为函数。非常感谢您的帮助。
【问题讨论】：
标签： language-agnostic geometry
这是我在 Python 中的解决方案。适用于 3d 点，您可以简化为 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)
用图片帮助可视化的测试示例：
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) #
【讨论】：
这是二维的吗？如果是这样，答案就是点 A 与线段 CD、B 与 CD、C 与 AB 或 D 与 AB 之间的最短距离。所以这是一个相当简单的“点和线之间的距离”计算（如果距离都相同，那么线是平行的）。
This site explains the algorithm for distance between a point and a line pretty well.
在 3 个维度上稍微复杂一些，因为线条不一定在同一平面上，但这里似乎不是这样？
【讨论】：
u 的值限制在[0,1] 范围内）。
取自this example，它还附带了一个简单的解释，说明它为什么能像 VB 代码一样工作（这比你需要的更多，所以我在翻译成 Python 时进行了简化——注意：我已经翻译了，但未经测试，因此可能会漏掉一个错字...）：
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)
【讨论】：
这是我的解决方案。它是用 Lua 编程的。它非常简洁，所以也许会受到赞赏。请确保线段的长度不为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
【讨论】：
我的解决方案是 Fnord 解决方案的翻译。我在 javascript 和 C 中做。
在 Javascript 中。您需要包含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);
纯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;
}
【讨论】：
为了计算 2 个 2D 线段之间的最小距离，您必须使用 4 个端点中的每一个依次执行从端点到其他线的 4 个垂直距离检查。但是，如果您发现绘制的垂直线在 4 种情况下都不与线段相交，那么您必须执行 4 次额外的端点到端点距离检查以找到最短距离。
我不知道是否有更优雅的解决方案。
【讨论】：
请注意，在假设线段不相交的情况下，上述解法是正确的！如果线段相交，很明显它们的距离应该为 0。因此有必要进行最后一次检查，即：假设点 A 和 CD 之间的距离 d(A,CD) 是上述 4 次检查中最小的由院长。然后从点 A 沿线段 AB 迈出一小步。记为点 E。如果 d(E,CD)
【讨论】：
这是我遵循的基本代码，用于任何两个计划或 3d 平面中的任何两个点之间的最短距离，它运行良好，可以针对给定的输入更改指标
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))
【讨论】：
【讨论】：
这个解决方案本质上是来自 Alex Martelli 的解决方案，但我添加了一个 Point 和 LineSegment 类以使阅读更容易。我还调整了格式并添加了一些测试。
线段交点错了，但对线段距离的计算似乎没有关系。如果您对正确的线段交点感兴趣，请看这里：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()
【讨论】：
我根据上面 Pratik Deoghare 的回答制作了一个 Swift 端口。 Pratik 引用了 Dan Sunday 的优秀文章和代码示例，可在此处找到：http://geomalgorithms.com/a07-_distance.html
以下函数计算两条线或两条线段之间的最小距离，是 Dan Sunday 的 C++ 示例的直接移植。
LASwift 线性代数包用于进行矩阵和向量计算。
//
// 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
}
【讨论】：
这是一个 Java 解决方案（通过点检查简单地完成，因此可能效率不高）：
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;
}
以及您需要的所有其他代码：
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);
}
【讨论】：
这是 perl 中的一个，与 Fnord 的有一些不同：
1e-6 值用于处理可能的浮点错误。如果您需要不同的容差，请调整或删除它们：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" ;
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}
【讨论】：