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) #
if t0 < 0 可能会变成 if t0 < 1e-6。 - KJ7LNW这段内容摘自这个例子，它提供了一个简单的解释和 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)
这是在二维平面上吗？如果是，答案就是点A和线段CD、B和CD、C和AB或D和AB之间的最短距离。因此这是一个相当简单的“点与线之间的距离”计算（如果所有距离都相同，则表示这些线段平行）。
在三维空间中稍微有些棘手，因为这些线不一定在同一个平面上，但这似乎不是这里的情况？
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);
#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;
}
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))
计算两个二维线段之间的最小距离，需要依次使用每个端点进行4次垂直距离检查。但是，如果您发现所绘制的垂线在这4种情况下都不与线段相交，则必须执行4个额外的端点到端点距离检查以找到最短距离。
我不知道是否有更优雅的解决方案。
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);
}
它返回RayA上最接近RayB的点，称为pointRayA，反之亦然。 一个简短的测试：
[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);
}
返回 (<0, 0, 0>, <1, 0, 0>)
我基于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
}