2つの線分の間の最短距離
2つの線分の間の最短距離を見つける関数が必要です。線分は2つの端点によって定義されます。したがって、たとえば、私の線分の1つ（AB）は2つの点A（x1、y1）とB（x2、y2）によって定義され、もう1つ（CD）は2つの点C（x1、y1）によって定義されます。およびD（x2、y2）。
好きな言語でソリューションを自由に書いてください。JavaScriptに翻訳できます。私の幾何学スキルはかなり錆びていることを覚えておいてください。私はすでに ここ を見てきましたが、これを関数に変換する方法がわかりません。助けてくれてありがとう。
これは2次元ですか？もしそうなら、答えは単に点Aと線分CD、BとCD、CとAB、またはDとABの間の距離の最短です。したがって、これはかなり単純な「点と線の間の距離」の計算です（距離がすべて同じである場合、線は平行です）。
このサイトでは、点と線の間の距離のアルゴリズムについて説明しています。
線が必ずしも同じ平面にあるとは限らないため、3次元では少し注意が必要ですが、ここではそうではないようです。
これはPythonでの私の解決策です。 3Dポイントで機能し、2Dで簡略化できます。
[編集1]結果を線分に制限したい場合は、クランプオプションを追加しました
[編集2] D.A。として2本の線が平行であるからといって、それらの間に距離を置くことができないという意味ではない、と指摘しました。そこで、その状況を処理するためにコードを編集しました。また、クランプ条件をより一般的にしたので、各セグメントをどちらの側でもクランプできます。
[編集3]両方のラインがクランプされた状態にあり、予測された結果がラインセグメントを超えた場合に発生する可能性があると指摘されたバグjhutarに対処しました。
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: (15.826771412132246, array([ 19.85163563, 26.21609078, 14.07303667]), array([ 26.99, 12.39, 11.18])) #
closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False)
# Result: (13.240709703623203, array([ 19.85163563, 26.21609078, 14.07303667]), array([ 18.40058604, 13.21580716, 12.02279907])) #
この例 から取得。これには、なぜ機能するのかについての簡単な説明と、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)
上記の解決策は、線分が交差しないことを前提として正しいことに注意してください。線分が交差する場合、それらの距離は0である必要があることは明らかです。したがって、次の1つの最終チェックが必要です。点AとCDの間の距離d（A、CD）が、前述の4つのチェックの中で最小であるとします。ディーンによる。次に、点AからセグメントABに沿って小さなステップを踏みます。この点Eを示します。d（E、CD）<d（A、CD）の場合、セグメントは交差している必要があります。これは、スティーブンが対処するケースには決してならないことに注意してください。
2つの2D線分の間の最小距離を計算するには、4つの端点のそれぞれを使用して、端点から他の線分までの4つの垂直距離を連続して実行する必要があるのは事実です。ただし、4つのケースのいずれにおいても、引き出された垂直線が線分と交差しない場合は、4つの追加の端点から端点までの距離チェックを実行して、最短距離を見つける必要があります。
これに対してもっと精力的な解決策があるかどうかはわかりません。
私のソリューションは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;
}
このソリューションは本質的にAlexMartelliのソリューションですが、読みやすくするためにPointクラスとLineSegmentクラスを追加しました。また、フォーマットを調整し、いくつかのテストを追加しました。
線分の交差は間違っていますが、線分の距離の計算には関係ないようです。正しい線分交差点に興味がある場合は、ここを見てください： 2つの線分が交差するかどうかをどのように検出しますか？
#!/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()
上記のPratikDeoghareの回答に基づいてSwiftポートを作成しました。Pratikは、DanSundayの優れた記事とコード例を参照しています： http://geomalgorithms.com/a07- _distance.html
次の関数は、2つの線または2つの線分の間の最小距離を計算し、DanSundayの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
}
これは、2つの平面または3D平面内の任意の2つのポイント間の最短距離について私が従う基本的なコードであり、適切に機能します。メトリックは、指定された入力に対して変更できます。
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))