文章目录
博文: 计算几何——判断两线段是否相交，提供了判断两线段是否相交的方法以及代码。然而，只是考虑了平面的情况。本博文提供一种简单有效的方法判断三维空间两线段是否相交，并提供完整matlab代码。
一、推导过程
设三维空间的线段
A
B
AB
AB与
C
D
CD
CD的端点分别为
(
x
1
,
y
1
,
z
1
)
,
(
x
2
,
y
2
,
z
2
)
(x_1,y_1,z_1), (x_2,y_2,z_2)
(x1,y1,z1),(x2,y2,z2) 和
(
x
3
,
y
3
,
z
3
)
,
(
x
4
,
y
4
,
z
4
)
(x_3,y_3,z_3), (x_4,y_4,z_4)
(x3,y3,z3),(x4,y4,z4)。
线段
A
B
AB
AB的参数方程为：
{
x
=
x
1
+
(
x
2
−
x
1
)
t
y
=
y
1
+
(
y
2
−
y
1
)
t
z
=
z
1
+
(
z
2
−
z
1
)
t
(1)
\begin{cases} x=x_1+(x_2-x_1)t\\ y=y_1+(y_2-y_1)t\\ z=z_1+(z_2-z_1)t\\ \tag 1 \end{cases}
⎩⎪⎨⎪⎧x=x1+(x2−x1)ty=y1+(y2−y1)tz=z1+(z2−z1)t(1)
其中，
t
∈
[
0
,
1
]
t\in[0,1]
t∈[0,1]。
线段
C
D
CD
CD的参数方程为：
{
x
=
x
3
+
(
x
4
−
x
3
)
t
′
y
=
y
3
+
(
y
4
−
y
3
)
t
′
z
=
z
3
+
(
z
4
−
z
3
)
t
′
(2)
\begin{cases} x=x_3+(x_4-x_3)t'\\ y=y_3+(y_4-y_3)t'\\ z=z_3+(z_4-z_3)t'\\ \tag 2 \end{cases}
⎩⎪⎨⎪⎧x=x3+(x4−x3)t′y=y3+(y4−y3)t′z=z3+(z4−z3)t′(2)
其中，
t
′
∈
[
0
,
1
]
t'\in[0,1]
t′∈[0,1]。
现在，两三维空间线段是否相交的问题转化为：是否存在
t
1
,
t
2
∈
[
0
,
1
]
t_1,t_2\in[0,1]
t1,t2∈[0,1], 使得下列方程组成立。
{
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
(3)
\begin{cases} 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\\ \tag 3 \end{cases}
⎩⎪⎨⎪⎧x1+(x2−x1)t1=x3+(x4−x3)t2y1+(y2−y1)t1=y3+(y4−y3)t2z1+(z2−z1)t1=z3+(z4−z3)t2(3)
方程组(3)写成矩阵形式:
a
[
t
1
t
2
]
=
b
(4)
a \left[ \begin{matrix} t_1 \\ t_2 \\ \end{matrix} \tag 4 \right] =b
a[t1t2]=b(4)
其中，
a
=
[
x
2
−
x
1
x
3
−
x
4
y
2
−
y
1
y
3
−
y
4
z
2
−
z
1
z
3
−
z
4
]
=
[
a
11
a
12
a
21
a
22
a
31
a
32
]
,
b
=
[
x
3
−
x
1
y
3
−
y
1
z
3
−
z
1
]
=
[
b
1
b
2
b
3
]
(5)
a = \left[ \begin{matrix} 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{matrix} \right] = \left[ \begin{matrix} a_{11} &a_{12} \\ a_{21} &a_{22} \\ a_{31} &a_{32} \\ \end{matrix} \right] , b= \left[ \begin{matrix} x_3-x_1 \\ y_3-y_1 \\ z_3-z_1 \\ \end{matrix} \right] =\left[ \begin{matrix} b_1 \\ b_2 \\ b_3 \\ \end{matrix} \right] \tag 5
a=⎣⎡x2−x1y2−y1z2−z1x3−x4y3−y4z3−z4⎦⎤=⎣⎡a11a21a31a12a22a32⎦⎤,b=⎣⎡x3−x1y3−y1z3−z1⎦⎤=⎣⎡b1b2b3⎦⎤(5)
1)当
a
T
a
a^Ta
aTa可逆(此时两线段存在交点或异面)时，利用最小二乘法求解方程组(4)得：
[
t
1
t
2
]
=
(
a
T
a
)
−
1
(
a
T
b
)
(6)
\left[ \begin{matrix} t_1 \\ t_2 \\ \end{matrix} \tag 6 \right] =(a^Ta)^{-1}(a^Tb)
[t1t2]=(aTa)−1(aTb)(6)
进一步，可以写成：
{
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
)
(7)
\begin{cases} 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})\\ \tag 7 \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧A11=a112+a212+a312A12=a11a12+a21a22+a31a32A21=A12A22=a122+a222+a322B1=a11b1+a21b2+a31b3B2=a12b1+a22b2+a32b3t1=−(A12B2−A22B1)/(A11A22−A12A21)t2=(A11B2−A21B1)/(A11A22−A12A21)(7)
其中，
A
11
A
22
−
A
12
A
21
≠
0
A_{11} A_{22} - A_{12} A_{21}\ne0
A11A22−A12A21=0。
解得
t
1
,
t
2
t_1,t_2
t1,t2后，判断其值是否在区间
[
0
,
1
]
[0,1]
[0,1]内，若是，则进一步判断下式是否成立：
{
∣
(
x
1
+
(
x
2
−
x
1
)
t
1
)
−
(
x
3
+
(
x
4
−
x
3
)
t
2
)
∣
<
e
p
s
∣
(
y
1
+
(
y
2
−
y
1
)
t
1
)
−
(
y
3
+
(
y
4
−
y
3
)
t
2
)
∣
<
e
p
s
∣
(
z
1
+
(
z
2
−
z
1
)
t
1
)
−
(
z
3
+
(
z
4
−
z
3
)
t
2
)
∣
<
e
p
s
(8)
\begin{cases} |(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\\ \tag 8 \end{cases}
⎩⎪⎨⎪⎧∣(x1+(x2−x1)t1)−(x3+(x4−x3)t2)∣<eps∣(y1+(y2−y1)t1)−(y3+(y4−y3)t2)∣<eps∣(z1+(z2−z1)t1)−(z3+(z4−z3)t2)∣<eps(8)
其中,eps为设定的容差。
若式(8)成立，则线段AB与线段CD相交，否则线段AB与线段CD异面。
2)当
a
T
a
a^Ta
aTa不可逆，也即
A
11
A
22
−
A
12
A
21
=
0
A_{11} A_{22} - A_{12} A_{21}=0
A11A22−A12A21=0时(此时两线段平行或共线)，分别判断点C是否在线段AB上，判断点D是否在线段AB上，判断点A是否在线段CD上，判断点B是否在线段CD上即可。
那么，如何判断三维空间任意一点是否在三维线段上呢？不失一般性，这里以判断点C是否在线段AB上为例。如下图容易得出：当点C不在线段AB上时，向量CA与向量CB夹角
θ
∈
(
0
,
180
°
)
\theta \in (0,180°)
θ∈(0,180°)；当点C在线段AB延长线上时，向量CA与向量CB夹角
θ
=
0
°
\theta =0°
θ=0°，当点C在线段AB上时，向量CA与向量CB夹角
θ
=
180
°
\theta =180°
θ=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 < tolerance
sta = 1;
else
cosTheta = dot(v1, v2) / normV1 / normV2;
if abs(cosTheta + 1.0) < EPS %两向量夹角为180度
sta = 1;
end
end
end
%{
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 == 1
disp('共线且有交集')
return;
end
%判断点D是否在线段AB上
sta = judge_point_on_line_segment(lineSegmentAB, lineSegmentCD.endPoint, tolerance);
if sta == 1
disp('共线且有交集')
return;
end
%判断点A是否在线段CD上
sta = judge_point_on_line_segment(lineSegmentCD, lineSegmentAB.startPoint, tolerance);
if sta == 1
disp('共线且有交集')
return;
end
%判断点B是否在线段CD上
sta = judge_point_on_line_segment(lineSegmentCD, lineSegmentAB.endPoint, tolerance);
if sta == 1
disp('共线且有交集')
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)) ) < tolerance
sta = 1;
end
end
end
end
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 == 0
disp('两线段不相交')
else
disp('两线段相交')
end
扫一扫
5893
到【灌水乐园】发言
