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

質問

座標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のライブラリを使わない方法の
サンプルソースの載っているページを教えていただけませんか？
また、ご迷惑でなければソースコードを記述していただけると助かります。

回答No.4

nineexit · ベストアンサー

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

お礼

わざわざFortranのプログラムを記述いただきありがとうございます。

本日、線分が半径を持つことが判明したため、
線分が衝突しないためには、
線分間で 半径*2 より大きい離隔距離が必要になりました
そのため　nineexitさんのコードを解析し、理解したく思います。

まず、
!!! 2つの直線の距離の2乗をdとする
!!! d=|((r1+s*r12)-(r3+t*r34))|^2
の部分がわからず、なぜこのように定義できるのか現在ネットで調査しています。

dot_product　はFortranの関数でベクトル間の内積を求めるということがわかりました、
以下のページを参考にエクセルVBAのコードに落とせるよう調べています。
http://homepage1.nifty.com/gfk/vector-naiseki.htm

まだまだ時間がかかりますが少しづつコードを解析していきます。

補足

質問者のGyustabです。

以下３点質問させてください。

(1)!!! 2つの直線の距離の2乗をdとする
　!!! d=|((r1+s*r12)-(r3+t*r34))|^2
　!!! dの最小値を求める
　の部分について　

　線分r1r2上の点を(r1+s*r12)
　線分r3r4上の点を(r3+t*r34) とすると

　２点間の距離は
　sqrt( ( (r3+t*r34)-(r1+s*r12) )^2 )　⇒　(r3+t*r34)-(r1+s*r12)　となり

　距離の２乗は　((r3+t*r34)-(r1+s*r12))^2 となるという理解で良いでしょうか？

(2)距離の２乗をdとしているのは、
　s,tについて微分出来る形にするためわざと２乗していると考えれば良いですか？

(3)　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)

　　上記数式がわかりません、
　　一連の流れは内積を使って何を計算しているのでしょうか？

回答No.6

nineexit

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

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

お礼

＞２つの直線上にある"２つの点"の距離の自乗がdです．
＞「２直線間の距離同士の内積」ではありません。
＞内積は距離同士でとるものではなくベクトル同士でとるものです。
＞同じベクトル同士の内積を取ると長さの自乗が得られるので，内積を取っています．
ご指摘ありがとうございます、数学素人の回答ですいません、
ベクトルと距離がごちゃまぜになっていました、
ご指摘いただいた点および今まで教えていただいた点を
含めて一度ソースコードを納得できるところまでコーディングしてみます。

回答No.5

nineexit

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

(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行列式です。
ただの連立一次方程式なので、行列やベクトルを持ち出す必要はありません。

お礼

返答ありがとうございます。

＞ベクトルa=(a1,a2,a3)の長さの自乗はa同士の内積（a・a）になります。
つまり、２直線間の距離の２乗であるdは、
２直線間の距離同士の内積と考えるということでしょうか？

＞d=sqrt(dot_product(rr,rr))
＞とありますが、これが長さの自乗を計算しているところです。
ベクトルa=(a1,a2,a3)の長さの自乗はa同士の内積（a・a）ということなので、
dot_product(rr,rr)はベクトルrrの長さの２乗と同じ意味ですね、
ということはベクトルrr同士の内積の平方根を求めるとベクトルrrの長さが出てくるという事ですね

＞微分しやすいように自乗しています。
＞２直線上の点の距離の最小値を求めるためには、
＞２直線上の点の距離の自乗の最小値を求めればよいので、このようなことをしています。
＞最後に最短距離を算出するときは、きちんとルートを取っています。
とてもわかりやすかったです、
紙上に数式を記述し、展開してs,tに対して微分したところ、
sとtについてまとめた数式まで辿り着けました。

回答No.3

nag0720

＃１です。

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

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

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

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

お礼

ご返答いただきありがとうございます。

＞前回の回答で、最初に「コードを書くのは面倒」と書いたのはそういう意味です。
＞悪しからず。
いえいえ、疑問に対する答えがわかっただけで助かりました。

明日、会社にて数式の許容誤差や
計算上の有効桁数の桁落ちをどのようにすれば良いかを熟考してみます。

回答No.2

imogasi

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

お礼

＞学校の宿題か何かの回答をここに丸投げ質問してないですか。そういうのは先般の大学入試問題にでも＞批判を浴びたはず。
＞もしそうなら、授業で先生に教えてもらえるのではないですか。それをここで教えるのは授業妨害とも＞言える。
いいえ違います社会人です、仕事上必要であったため質問しました、
教えてくれる先生などいませんし、授業を妨害している暇はありません。

＞Ｇｏｏｇｌｅででも「空間　２直線　交点座標　」などで照会すれば記事はある。
「線分　交点　３次元」、「直線　衝突判定　３次元」、「直線　交点　サンプルプログラム」等で
検索しましたが、概念はたくさんあっても私の解読できるC言語・VisualBasic・Fortran・javaのソースが発見できなかったためここに質問しました。
客先にはエクセルで提供するのでDirectXやOpenGLのようなライブラリを要するものは使用したくないのです。

＞ベクトルや行列の知識が必要な記事が多いが。そういうのは理解できるのか。
数学者でも教師でも数値解析エンジニアでもないのでベクトルや行列について１～１００まで
理解する気はありません、客先の仕様を満たすために必要な概念と考え方、
ソースコードへ落とす事が必要なだけです。

以上、なにか返答があればどうぞ

回答No.1

nag0720

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

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 の場合、２直線は、同じか、並行か、ねじれ位置にあるので交差していないことになります。

お礼

返信ありがとうございます、以下の部分が大変助かりました。

s=((x3-x1)*y34-x34*(y3-y1))/(x12*y34-x34*y12)
t=((x1-x3)*y12-x12*(y1-y3))/(x34*y12-x12*y34)

さまざまなホームページで紹介されている概念の通りにやれば紙の上では解けるのですが、
どうやって２線分の交点をソースコードに落とせば良いかで四苦八苦しておりました。

今よりコーディングしてみます。
補足もありがとうございます、この部分も条件に組み込みます。

補足

質問者のGyustabです。

コーディング中疑問に思ったため再度質問させてください。

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

成立しないのであれば (x12*y34)/(x34*y12)≒1.00000 のように限りなく１に近づく誤差を
計算したり、s=・・・・やt=・・・の式の乗算・除算の順番を考え有効桁数の
精度を考慮した計算をする必要があるのではないかと思っています。

数式１：s=((x3-x1)*y34-x34*(y3-y1))/(x12*y34-x34*y12)
数式２：t=((x1-x3)*y12-x12*(y1-y3))/(x34*y12-x12*y34)
数式３：z1+s*z12=z3+t*z34
数式４：x12*y34=x34*y12