問題を定義する
２つの線分
- (x1, y1, z1)から(x1 + dx1, y1 + dy1, z1 + dz1)への線分
- (x2, y2, z2)から(x2 + dx2, y2 + dy2, z2 + dz2)への線分 の間の最短距離について考える。
問題を整理する
言い換えれば、２点
- (x1 + t1 * dx1, y1 + t1 * dy1, z1 + t1 * dz1)
- (x2 + t2 * dx2, y2 + t2 * dy2, z2 + t2 * dz2)
の最短距離でもある。 （線分の区間に収まるならば、0 <= t1 <= 1 かつ 0 <= t2 <= 1）
２つの線の最短経路のベクトルと線１の内積は0なので
( (x2 + t2*dx2) - (x1 + t1*dx1) ) * dx1 + ( (y2 + t2*dy2) - (y1 + t1*dy1) ) * dy1 + ( (z2 + t2*dz2) - (z1 + t1*dz1) ) * dz1 = 0
よって
t1*(dx1*dx1 + dy1*dy1 + dz1*dz1) = x2*dx1 + t2*dx1*dx2 - x1*dx1 + y2*dy1 + t2*dy1*dy2 - y1*dy1 + z2*dz1 + t2*dz1*dz2 - z1*dz1
点ではなく線分なら(dx1*dx1 + dy1*dy1 + dz1*dz1)は0ではない。 両辺を割ると
t1 = (x2*dx1 + t2*dx1*dx2 - x1*dx1 + y2*dy1 + t2*dy1*dy2 - y1*dy1 + z2*dz1 + t2*dz1*dz2 - z1*dz1) / (dx1*dx1 + dy1*dy1 + dz1*dz1)
最短経路のベクトルと線２の内積も0なので ( (x2 + t2*dx2) - (x1 + t1*dx1) ) * dx2 + ( (y2 + t2*dy2) - (y1 + t1*dy1) ) * dy2 + ( (z2 + t2*dz2) - (z1 + t1*dz1) ) * dz2 = 0
よって
t1*(dx1*dx2 + dy1*dy2 + dz1*dz2) = x2*dx2 + t2*dx2*dx2 - x1*dx2 + y2*dy2 + t2*dy2*dy2 - y1*dy2 + z2*dz2 + t2*dz2*dz2 - z1*dz2
ここで線１との内積から求めた式を持ってきて
(x2*dx1 + t2*dx1*dx2 - x1*dx1 + y2*dy1 + t2*dy1*dy2 - y1*dy1 + z2*dz1 + t2*dz1*dz2 - z1*dz1)*(dx1*dx2 + dy1*dy2 + dz1*dz2) / (dx1*dx1 + dy1*dy1 + dz1*dz1) = x2*dx2 + t2*dx2*dx2 - x1*dx2 + y2*dy2 + t2*dy2*dy2 - y1*dy2 + z2*dz2 + t2*dz2*dz2 - z1*dz2
よって
t2* ( (dx1*dx2 + dy1*dy2 + dz1*dz2)*(dx1*dx2 + dy1*dy2 + dz1*dz2)
- (dx2*dx2 + dy2*dy2 + dz2*dz2)*(dx1*dx1 + dy1*dy1 + dz1*dz1) ) =
- (x2*dx2 - x1*dx2 + y2*dy2 - y1*dy2 + z2*dz2 - z1*dz2)*(dx1*dx1 + dy1*dy1 + dz1*dz1)
- (x2*dx1 - x1*dx1 + y2*dy1 - y1*dy1 + z2*dz1 - z1*dz1)*(dx1*dx2 + dy1*dy2 + dz1*dz2)
２つの線分が平行でなければ
t2 = ( (x2*dx2 - x1*dx2 + y2*dy2 - y1*dy2 + z2*dz2 - z1*dz2)*(dx1*dx1 + dy1*dy1 + dz1*dz1)
- (x2*dx1 - x1*dx1 + y2*dy1 - y1*dy1 + z2*dz1 - z1*dz1)*(dx1*dx2 + dy1*dy2 + dz1*dz2) ) / ( (dx1*dx2 + dy1*dy2 + dz1*dz2)*(dx1*dx2 + dy1*dy2 + dz1*dz2)
- (dx2*dx2 + dy2*dy2 + dz2*dz2)*(dx1*dx1 + dy1*dy1 + dz1*dz1) )
例外的な状況
但し dx1*dx1 + dy1*dy1 + dz1*dz1 = 0 ならば線分１の長さは0で、点からの距離と考えるべき。
また (dx1*dx2 + dy1*dy2 + dz1*dz2)*(dx1*dx2 + dy1*dy2 + dz1*dz2) - (dx2*dx2 + dy2*dy2 + dz2*dz2)*(dx1*dx1 + dy1*dy1 + dz1*dz1) = 0 ならば２つの線分は平行で、最短経路を特定することができない。
実装
これからする。
最適化
まだやってない。
