文章目錄

• 一、推導過程
• 二、MATLAB代碼

??博文: 計算幾何——判斷兩線段是否相交，提供了判斷兩線段是否相交的方法以及代碼。然而，只是考慮了平面的情況。本博文提供一種簡單有效的方法判斷三維空間兩線段是否相交，并提供完整matlab代碼。

一、推導過程

??設三維空間的線段$AB$與$CD$的端點分別為 $(x_1,y_1,z_1),(x_2,y_2,z_2)$ 和$(x_3,y_3,z_3),(x_4,y_4,z_4)$。
線段$AB$的參數方程為：
$$?\begin{array}{l}x=x_1+(x_2?x_1)t\\ y=y_1+(y_2?y_1)t\\ z=z_1+(z_2?z_1)t\end{array}\text{\hspace{1em}(1)}$$
??其中，$t\in [0,1]$。
線段$CD$的參數方程為：
$$?\begin{array}{l}x=x_3+(x_4?x_3)t^{\prime }\\ y=y_3+(y_4?y_3)t^{\prime }\\ z=z_3+(z_4?z_3)t^{\prime }\end{array}\text{\hspace{1em}(2)}$$
??其中，$t^{\prime }\in [0,1]$。
??現在，兩三維空間線段是否相交的問題轉化為：是否存在$t_1,t_2\in [0,1]$, 使得下列方程組成立。
$$?\begin{array}{l}{x}_1+(x_2?x_1)t_1=x_3+(x_4?x_3)t_2\\ y_1+(y_2?y_1)t_1=y_3+(y_4?y_3)t_2\\ z_1+(z_2?z_1)t_1=z_3+(z_4?z_3)t_2\end{array}\text{\hspace{1em}(3)}$$
??方程組(3)寫成矩陣形式:
$$a\left[\begin{array}{c}{t}_1\\ t_2\end{array}\right]=b\text{\hspace{1em}(4)}$$
??其中，$$a=?\begin{array}{cc}{x}_2?x_1& x_3?x_4\\ y_2?y_1& y_3?y_4\\ z_2?z_1& z_3?z_4\end{array}?=?\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\\ a_{31}& a_{32}\end{array}?,b=?\begin{array}{c}{x}_3?x_1\\ y_3?y_1\\ z_3?z_1\end{array}?=?\begin{array}{c}{b}_1\\ b_2\\ b_3\end{array}?\text{\hspace{1em}(5)}$$
??1)當$a^{T}a$可逆(此時兩線段存在交點或異面)時，利用最小二乘法求解方程組(4)得：
$$\left[\begin{array}{c}{t}_1\\ t_2\end{array}\right]=(a^{T}a{)}^{?1}(a^{T}b)\text{\hspace{1em}(6)}$$
??進一步，可以寫成：
$$?\begin{array}{l}{A}_{11}=a_{11}^2+a_{21}^2+a_{31}^2\\ A_{12}=a_{11}{a}_{12}+a_{21}{a}_{22}+a_{31}{a}_{32}\\ A_{21}=A_{12}\\ A_{22}=a_{12}^2+a_{22}^2+a_{32}^2\\ B_1=a_{11}{b}_1+a_{21}{b}_2+a_{31}{b}_3\\ B_2=a_{12}{b}_1+a_{22}{b}_2+a_{32}{b}_3\\ t_1=?(A_{12}{B}_2?A_{22}{B}_1)/(A_{11}{A}_{22}?A_{12}{A}_{21})\\ t_2=(A_{11}{B}_2?A_{21}{B}_1)/(A_{11}{A}_{22}?A_{12}{A}_{21})\end{array}\text{\hspace{1em}(7)}$$
??其中，$A_{11}{A}_{22}?A_{12}{A}_{21}=0$。
??解得$t_1,t_2$后，判斷其值是否在區間$[0,1]$內，若是，則進一步判斷下式是否成立：
$$?\begin{array}{l}|(x_1+(x_2?x_1)t_1)?(x_3+(x_4?x_3)t_2)|<eps\\ |(y_1+(y_2?y_1)t_1)?(y_3+(y_4?y_3)t_2)|<eps\\ |(z_1+(z_2?z_1)t_1)?(z_3+(z_4?z_3)t_2)|<eps\end{array}\text{\hspace{1em}(8)}$$
??其中,eps為設定的容差。
??若式(8)成立，則線段AB與線段CD相交，否則線段AB與線段CD異面。
??2)當$a^{T}a$不可逆，也即$A_{11}{A}_{22}?A_{12}{A}_{21}=0$時(此時兩線段平行或共線)，分別判斷點C是否在線段AB上，判斷點D是否在線段AB上，判斷點A是否在線段CD上，判斷點B是否在線段CD上即可。
??那么，如何判斷三維空間任意一點是否在三維線段上呢？不失一般性，這里以判斷點C是否在線段AB上為例。如下圖容易得出：當點C不在線段AB上時，向量CA與向量CB夾角$\theta \in (0,180°)$；當點C在線段AB延長線上時，向量CA與向量CB夾角$\theta =0°$，當點C在線段AB上時，向量CA與向量CB夾角$\theta =180°$。

二、MATLAB代碼

%{ Function: judge_point_on_line_segment Description: 判斷三維空間任意一點是否在三維線段上 Input: 三維線段lineSegment(包含起點與終點坐標), 三維空間任意一點point, 容差tolerance Output: 狀態sta(1表示點在線段上，0表示不在線段上) Author: Marc Pony(marc_pony@163.com) %} function sta = judge_point_on_line_segment(lineSegment, point, tolerance)x1 = lineSegment.startPoint(1); y1 = lineSegment.startPoint(2); z1 = lineSegment.startPoint(3);x2 = lineSegment.endPoint(1); y2 = lineSegment.endPoint(2); z2 = lineSegment.endPoint(3);v1 = [x1 - point(1); y1 - point(2); z1 - point(3)]; v2 = [x2 - point(1); y2 - point(2); z2 - point(3)]; normV1 = norm(v1, 2); normV2 = norm(v2, 2); EPS = 1.0e-8; sta = 0; if normV1 < tolerance || normV2 < tolerancesta = 1; elsecosTheta = dot(v1, v2) / normV1 / normV2;if abs(cosTheta + 1.0) < EPS %兩向量夾角為180度sta = 1;end endend %{ Function: judge_two_line_segment_intersection Description: 判斷兩段三維線段是否相交(共線且有交集也認為是相交) Input: 三維線段lineSegmentAB(包含起點與終點坐標), 三維線段lineSegmentCD(包含起點與終點坐標), 容差tolerance Output: 狀態sta(1表示兩段三維線段相交，0表示兩段三維線段不相交) Author: Marc Pony(marc_pony@163.com) %} function sta = judge_two_line_segment_intersection(lineSegmentAB, lineSegmentCD, tolerance) x1 = lineSegmentAB.startPoint(1); y1 = lineSegmentAB.startPoint(2); z1 = lineSegmentAB.startPoint(3); x2 = lineSegmentAB.endPoint(1); y2 = lineSegmentAB.endPoint(2); z2 = lineSegmentAB.endPoint(3);x3 = lineSegmentCD.startPoint(1); y3 = lineSegmentCD.startPoint(2); z3 = lineSegmentCD.startPoint(3); x4 = lineSegmentCD.endPoint(1); y4 = lineSegmentCD.endPoint(2); z4 = lineSegmentCD.endPoint(3);a11 = x2 - x1; a12 = x3 - x4; b1 = x3 - x1;a21 = y2 - y1; a22 = y3 - y4; b2 = y3 - y1;a31 = z2 - z1; a32 = z3 - z4; b3 = z3 - z1;A11 = a11^2 + a21^2 + a31^2; A12 = a11 * a12 + a21 * a22 + a31 * a32; A21 = A12; A22 = a12^2 + a22^2 + a32^2; B1 = a11 * b1 + a21 * b2 + a31 * b3; B2 = a12 * b1 + a22 * b2 + a32 * b3;EPS = 1.0e-10; sta = 0; temp = A11 * A22 - A12 * A21; if abs(temp) < EPS %平行或共線%判斷點C是否在線段AB上sta = judge_point_on_line_segment(lineSegmentAB, lineSegmentCD.startPoint, tolerance);if sta == 1disp('共線且有交集')return;end%判斷點D是否在線段AB上sta = judge_point_on_line_segment(lineSegmentAB, lineSegmentCD.endPoint, tolerance);if sta == 1disp('共線且有交集')return;end%判斷點A是否在線段CD上sta = judge_point_on_line_segment(lineSegmentCD, lineSegmentAB.startPoint, tolerance);if sta == 1disp('共線且有交集')return;end%判斷點B是否在線段CD上sta = judge_point_on_line_segment(lineSegmentCD, lineSegmentAB.endPoint, tolerance);if sta == 1disp('共線且有交集')return;end else %異面或相交t = [-(A12 * B2 - A22 * B1) / temp, (A11 * B2 - A21 * B1) / temp];if (t(1) >= 0 - EPS && t(1) <= 1.0 + EPS) && (t(2) >= 0 - EPS && t(2) <= 1.0 + EPS)if abs( (x1 + (x2 - x1) * t(1)) - (x3 + (x4 - x3) * t(2)) ) < tolerance ...&& abs( (y1 + (y2 - y1) * t(1)) - (y3 + (y4 - y3) * t(2)) ) < tolerance ...&& abs( (z1 + (z2 - z1) * t(1)) - (z3 + (z4 - z3) * t(2)) ) < tolerancesta = 1;endend endend clc clear close all%{ syms a11 a12 a21 a22 a31 a32 b1 b2 b3 real syms A11 A12 A21 A22 B1 B2 real A = [a11 a12;a21 a22;a31 a32]; B = [b1;b2;b3]; (A'*A), (A'*B) t = (A'*A)\(A'*B); %}axis([0 10 0 10]) [x, y] = ginput(4); plot([x(1) x(2)], [y(1) y(2)]) hold on plot([x(3) x(4)], [y(3) y(4)]) axis([0 10 0 10])line1.startPoint = [x(1) y(1) 0]; line1.endPoint = [x(2) y(2) 0]; line2.startPoint = [x(3) y(3) 0]; line2.endPoint = [x(4) y(4) 0];tolerance = 1.0e-6; sta = judge_two_line_segment_intersection(line1, line2, tolerance); if sta == 0disp('兩線段不相交') elsedisp('兩線段相交') end

總結

以上是生活随笔為你收集整理的判断三维空间两线段是否相交(附代码)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。