３次元座標上の２直線の交点判定について

Question

座標A(x1,y1,z1)から座標B(x2,y2,z2)への線分ABと
座標C(x3,y3,z3)から座標B(x4,y4,z4)への線分CDがあり、
線分ABと線分CDが交点を持つかどうかのプログラムを作りたいです。

C言語かVBかFortranで記述され、DirectXやOpenGLのライブラリを使わない方法の
サンプルソースの載っているページを教えていただけませんか？
また、ご迷惑でなければソースコードを記述していただけると助かります。

nineexit · Accepted Answer

2線分の最短距離を求めてそれが0になる場合，交わっていると判断するのが良いと思います．
piyoは2線分の最短距離を求めるサブルーチンです．
2線分が平行なときは別の処理が必要ですね．

subroutine piyo(r1,r2,r3,r4,d)
  implicit none
  real(8),intent(in)::r1(3),r2(3),r3(3),r4(3)
  real(8),intent(out)::d
  real(8) r12(3),r34(3),b(2),a(2,2),s,t,rr(3),deta
  r12(:)=r2(:)-r1(:)
  r34(:)=r4(:)-r3(:)
  
!!! 2つの直線の距離の2乗をdとする
!!! d=|((r1+s*r12)-(r3+t*r34))|^2
!!! dの最小値を求める
!!! dをsについて偏微分
!!! r12・((r1+s*r12)-(r3+t*r34))=0
!!! dをtについて偏微分
!!! -r34・((r1+s*r12)-(r3+t*r34))=0
!!! sとtについてまとめると
!!! r12・r12 s - r12・r34 t = -r12・r1 +r12・r3
!!! -r34・r12 s + r34・r34 t = r34・r1 -r34・r3
  a(1,1)=dot_product(r12,r12)
  a(2,1)=-dot_product(r12,r34)
  a(1,2)=a(2,1)
  a(2,2)=dot_product(r34,r34)
  b(1)=-dot_product(r12,r1)+dot_product(r12,r3)
  b(2)=dot_product(r34,r1)-dot_product(r34,r3)
  deta=a(1,1)*a(2,2)-a(1,2)*a(2,1)
!!!  sとtについて解く
  if(deta /=0.d0) then
     s=(a(2,2)*b(1)-a(1,2)*b(2))/deta
     t=(-a(2,1)*b(1)+a(1,1)*b(2))/deta
!!!  elseif
!!! deta=0の場合は別途処理する必要あり．2つの線分が平行な場合deta=0となる．
  end if

!!! この時点で0<s<1, 0<t<1でなければ，交わっていないことが分かる．
!!!  if(s<0.d0.or.1.d0<s.or.t<0.d0.or.1.d0<t) then
!!!     write(*,*) "交わってない"
!!!  stop

if(s<0.d0) then
     s=0.d0
  elseif(1.d0<s)then
     s=1.d0
  end if
  if(t<0.d0) then
     t=0.d0
  elseif(1.d0<t)then
     t=1.d0
  end if

!!! 最短距離の計算
  rr(:)=r1(:)+s*r12(:)-(r3(:)+t*r34(:))
  d=sqrt(dot_product(rr,rr))
end subroutine piyo

program hoge
  implicit none
  real(8) r1(3),r2(3),r3(3),r4(3),d
  r1(:)=(/0.d0,0.d0,0.d0/)
  r2(:)=(/1.d0,1.d0,1.d0/)
  r3(:)=(/1.d0,0.d0,0.d0/)
  r4(:)=(/1.d0,1.d0,0.d0/)
  call piyo(r1,r2,r3,r4,d)
  if(d<1.d-15) then
     write(*,*) "交わる"
  endif
end program

nineexit · Answer

> つまり、２直線間の距離の２乗であるdは、
> ２直線間の距離同士の内積と考えるということでしょうか？

２つの直線上にある"２つの点"の距離の自乗がdです．
「２直線間の距離同士の内積」ではありません．
内積は距離同士でとるものではなくベクトル同士でとるものです．
同じベクトル同士の内積を取ると長さの自乗が得られるので，内積を取っています．

nineexit · Answer

回答に対する質問への回答です。

(1) 
r1, r2, r3, r4, r12, r34はベクトルです。
ベクトルa=(a1,a2,a3)の長さの自乗はa同士の内積（a・a）になります。
サブルーチンの終わりに
rr(:)=r1(:)+s*r12(:)-(r3(:)+t*r34(:))
d=sqrt(dot_product(rr,rr))
とありますが、これが長さの自乗を計算しているところです。

(2)
微分しやすいように自乗しています。
２直線上の点の距離の最小値を求めるためには、２直線上の点の距離の自乗の最小値を求めればよいので、このようなことをしています。
最後に最短距離を算出するときは、きちんとルートを取っています。

(3)
!!! r12・r12 s - r12・r34 t = -r12・r1 +r12・r3
!!! -r34・r12 s + r34・r34 t = r34・r1 -r34・r3
を行列A、ベクトルbを用いて表記しました。
Ax=bの形でxはsとtからなるベクトルです。x=(s,t)
detaは行列A行列式です。
ただの連立一次方程式なので、行列やベクトルを持ち出す必要はありません。

nag0720 · Answer

＃１です。

＞下記４つの数式の条件を満たすのは４点の座標の値が整数の場合のみでしょうか？
＞座標の値が単精度浮動小数点や倍精度浮動小数点の場合は有効桁数の範囲内であれば成立しますか？

数学的には、実数の範囲で成立します。

コンピュータで単精度、倍精度で計算する場合は当然有効桁数の問題がでてきます。
特に、減算で有効桁数が桁落ちする可能性がでてくるので、十分考慮する必要があります。

前回の回答で、最初に「コードを書くのは面倒」と書いたのはそういう意味です。悪しからず。

imogasi · Answer

学校の宿題か何かの回答をここに丸投げ質問してないですか。そういうのは先般の大学入試問題にでも批判を浴びたはず。
もしそうなら、授業で先生に教えてもらえるのではないですか。それをここで教えるのは授業妨害とも言える。
Ｇｏｏｇｌｅででも「空間　２直線　交点座標　」などで照会すれば記事はある。
ベクトルや行列の知識が必要な記事が多いが。そういうのは理解できるのか。
http://www.teu.ac.jp/aqua/GS/text/PDF/3DMath.pdf

nag0720 · Answer

コードを書くのは面倒なので考え方だけ。

x12=x2-x1、y12=y2-y1、z12=z2-z1、
x34=x4-x3、y34=y4-y3、z34=z4-z3とすると、
線分AB上の点は(x1+s*x12, y1+s*y12, z1+s*z12) （0≦s≦1）
線分CD上の点は(x3+t*x34, y3+t*y34, z3+t*z34) （0≦t≦1）
と表現できます。

線分ABと線分CDが交点を持つとすると、
(1)　x1+s*x12=x3+t*x34
(2)　y1+s*y12=y3+t*y34
(3)　z1+s*z12=z3+t*z34
が成り立ちます。

(1),(2)式からs,tを求めると、
s=((x3-x1)*y34-x34*(y3-y1))/(x12*y34-x34*y12)
t=((x1-x3)*y12-x12*(y1-y3))/(x34*y12-x12*y34)

このS,tが0≦s≦1 , 0≦t≦1　の条件を満たし、かつ、
このs,tを(3)式に代入して等号が成立するときに交点を持ちます。

あとは、これをコードにすればいいだけです。

補足
x12*y34=x34*y12 の場合、２直線は、同じか、並行か、ねじれ位置にあるので交差していないことになります。

３次元座標上の２直線の交点判定について

2線分の最短距離を求めてそれが0になる場合，交わっていると判断するのが良いと思います．

この回答への補足

> つまり、２直線間の距離の２乗であるdは、

回答に対する質問への回答です。

＃１です。

学校の宿題か何かの回答をここに丸投げ質問してないですか。

コードを書くのは面倒なので考え方だけ。

この回答への補足

似たような質問が見つかりました

このQ&Aを見た人はこんなQ&Aも見ています

関連するカテゴリからQ&Aを探す

このQ&Aを見た人がよく見るQ&A

デイリーランキングこのカテゴリの人気デイリーQ&Aランキング

マンスリーランキングこのカテゴリの人気マンスリーQ&Aランキング