直線と原点との距離 3次元版

id:yellow_73:20060613 では2次元についてやっていましたが、今度は3次元。
まず、p0(x0,y0,z0)-p1(x1,y1,z1) を結ぶ直線と原点との距離を計算します。
直線の式は

(x-x0)/dx = (y-y0)/dy = (z-z0)/dz (ただしdx=x1-x0,dy=y1-y0,dz=z1-z0)

ここで

t = (x-x0)/dx = (y-y0)/dy = (z-z0)/dz

とすると、

x = dx*t + x0
y = dy*t + y0
z = dz*t + z0

です。ここで

x^2+y^2+z^2=r^2

に代入すると、次のようになります。

(dx^2+dy^2+dz^2)*t^2 - 2*(dx*x0+dy*y0+dz*z0) + x0^2+y0^2+z0^2-r^2=0 ... (A)

tに関する方程式の判別式をDとすると、

D=(dx*x0+dy*y0+dz*z0)^2-(dx^2+dy^2+dz^2)*(x0^2+y0^2+z0^2-r^2)

D=0となるときにrが求める距離になるので、

(dx*x0+dy*y0+dz*z0)^2-(dx^2+dy^2+dz^2)*(x0^2+y0^2+z0^2-r^2)=0

これをr^2について解くと、

r^2 = ((dx*y0-dy*x0)^2+(dy*z0-dz*y0)^2+(dz*x0-dx*z0)^2)/(dx^2+dy^2+dz^2)

ちなみに、id:yellow_73:20060613 にあわせると、a=dy, b=dx, c=dy*x0-dx*y0 で、同じ結果になります。
次に、交点が、[p0,p1]の区間にあるかをチェックします。交点を求めようと思ったけれどもそれはやめて、上で使った t を使います。
D=0となるとき、(A)をtについて解くと、

t=(dx*x0+dy*y0+dz*z0)/(dx^2+dy^2+dz^2)

となります。このtは、交点が区間内にあるとき 0 <= t <= 1 となります。
なお、交点を求めたい場合は、このあと、x=dx*t+x0 から求めていきます。