直線同士の距離を求める

2本の直線を最短で結ぶ直線の交点を求めて距離を計算する。

計算

直線ABとCDを最短で結ぶベクトルはどちらの直線とも90度で存在している。どちらの直線とも内積が０で交わると考えて方程式を立てて解く。

正規化したベクトルABをab、正規化したベクトルCDをcdとする。AからP1までの距離をd1、CからP2までの距離をd2とする。

P1 = A + d1 * ab
P2 = C + d2 * cd

ベクトルP1P2は

P2 – P1 = C + d2 * cd – (A + d1 * ab) = C – A + d2 * cd – d1 * ab

ベクトルACをacとする

P1P2 = ac + d2 * cd – d1 * ab

ベクトルAB、ベクトルCDとの内積の式を立てると

ab・(ac + d2 * cd – d1 * ab) = 0
cd・(ac + d2 * cd – d1 * ab) = 0

式を展開すると

ab・ac + d2(ab・cd) – d1(ab・ab) = 0

abは正規化ベクトルなのでab・abは１になる

d1 = ab・ac + d2(ab・cd)

同じように

cd・(ac + d2 * cd – d1 * ab) = 0

cd・ac + d2(cd・cd) – d1(cd・ab) = 0

d2 = d1(ab・cd) – cd・ac

この２つの式をそれぞれ代入してまとめる

d1 = ab・ac + (d1(ab・cd) – cd・ac)(ab・cd)

d1 = (ab・ac) + d1(cd・ab)(ab・cd) – (cd・ac)(ab・cd)

d1 – d1(ab・cd)(ab・cd) = (ab・ac) – (cd・ac)(ab・cd)

d1( 1 – (ab・cd)(ab・cd) ) = (ab・ac) – (cd・ac)(ab・cd)

d1 = ( (ab・ac) – (cd・ac)(ab・cd) ) / ( 1 – (ab・cd)(ab・cd) )

同じようにd2をまとめると

d2 = ( ab・ac + d2(ab・cd) )(cd・ab) – cd・ac

d2 = (ab・ac)(ab・cd) + d2(ab・cd)(ab・cd) – (cd・ac)

d2 – d2(ab・cd)(ab・cd) = (ab・ac)(ab・cd) – (cd・ac)

d2 ( 1 – (ab・cd)(ab・cd) ) = (ab・ac)(ab・cd) – (cd・ac)

d2 = ( (ab・ac)(ab・cd) – (cd・ac) ) / ( 1 – (ab・cd)(ab・cd) )

となる

コード

// RunOver: Detail

vector a = point(1, "P", 0);
vector b = point(1, "P", 1);

vector ab = normalize(b - a);

vector c = point(2, "P", 0);
vector d = point(2, "P", 1);

vector cd = normalize(d - c);

// ベクトルACは正規化しない
vector ac = c - a;

float d1 = (dot(ab, ac) - dot(ac, cd) * dot(ab, cd)) / (1 - dot(ab, cd) * dot(ab, cd));

vector p1 = a + ab * d1;
int pt1 = addpoint(0, p1);

float d2 = (dot(ab, ac) * dot(ab, cd) - dot(cd, ac)) / (1 - dot(ab, cd) * dot(ab, cd));

vector p2 = c + cd * d2;
int pt2 = addpoint(0, p2);

// ポリラインを生成
int prim = addprim(0, "polyline");
addvertex(0, prim, pt1);
addvertex(0, prim, pt2);

setpointattrib(0, "Cd", pt1, set(1,0.5,0));
setpointattrib(0, "Cd", pt2, set(1,0.5,0));

//printf('d1:' + sprintf('%g', d1) + '\n');