線分と点の距離 - ゲームプログラミングというゲームを攻略するWIKI

問題を定義する

• (x1, y1, z1)から(x1 + dx1, y1 + dy1, z1 + dz1)への線分
• (x2, y2, z2)の点

の間の距離を考える。

問題を整理する

線分上を点が移動する間にtが0から1に変化すると考えてもよい。 線分が点に最も近くなる箇所を(x, y, z)とする。

(x, y, z) = (x1 + t*dx1, y1 + t*dy1, z1 + t*dz1)

(dx1, dy1, dz1)と(x - x2, y - y2, z - z2)は直交するので

dx1*(x - x2) + dy1*(y - y2) + dz1*(z - z2) = 0

ゆえに

dx1*(x1 + t*dx1 - x2) + dy1*(y1 + t*dy1 - y2) + dz1*(z1 + t*dz1 - z2) = 0

dx1*x1 - dx1*x2 + dy1*y1 - dy1*y2 + dz1*z1 - dz1*z2 + t*(dx1*dx1 + dy1*dy1 + dz1*dz1) = 0

t*(dx1*dx1 + dy1*dy1 + dz1*dz1) = dx1*x2 - dx1*x1 + dy1*y2 - dy1*y1 + dz1*z2 - dz1*z1

t*(dx1*dx1 + dy1*dy1 + dz1*dz1) = dx1*(x2 - x1) + dy1*(y2 - y1) + dz1*(z2 - z1)

t = (dx1*(x2 - x1) + dy1*(y2 - y1) + dz1*(z2 - z1)) / (dx1*dx1 + dy1*dy1 + dz1*dz1)

例外的な状況

但し dx1*dx1 + dy1*dy1 + dz1*dz1 = 0 ならば線分の長さは0で、点からの距離と考えるべき。

t < 0 ならば最も近くなる箇所にまだ到達していないので、始点からの距離と考えるべき。

1 < t ならば最も近くなる箇所はもう通過しているので、終点からの距離と考えるべき

実装

というわけで以下のようなコードになる。

double distance(double x1, double y1, double z1, double dx1, double dy1, double dz1, double x2, double y2, double z2)
{
	double div = dx1 * dx1 + dy1 * dy1 + dz1 * dz1;
	if (div == 0)
	{
		return distance(x1, y1, z1, x2, y2, z2);
	}
 
	double t = (dx1 * (x2 - x1) + dy1 * (y2 - y1) + dz1 * (z2 - z1)) / div;
 
	if (t < 0)
	{
		return distance(x1, y1, z1, x2, y2, z2);
	}
 
	if (1 < t)
	{
		return distance(x1 + dx1, y1 + dy1, z1 + dz1, x2, y2, z2);
	}
 
	return distance(x1 + t * dx1, y1 + t * dy1, z1 + t * dz1, x2, y2, z2);
}

最適化

• 浮動小数点の誤差を考えれば、divが0より大きくても0に近いなら0同様に処理したほうがよい。たぶん。
• t < 0はt == 0の場合を含めてt <= 0としても結果は同じだし、若干速くなる。
• 1 < tはt == 1の場合を含めて1 <= tとしても結果は同じだし、若干速くなる。