判断三维空间两线段是否相交(附代码)

最新推荐文章于 2023-02-03 16:21:37 发布

Marc Pony

最新推荐文章于 2023-02-03 16:21:37 发布

阅读量3.9k

点赞数 5

分类专栏：计算几何文章标签：判断三维空间两线段是否相交两线段相交计算几何空间两线段相交判断

本文链接：https://blog.csdn.net/maple_2014/article/details/124001002

版权

计算几何专栏收录该内容

11 篇文章 11 订阅

订阅专栏

文章目录

一、推导过程
二、MATLAB代码

博文: 计算几何——判断两线段是否相交，提供了判断两线段是否相交的方法以及代码。然而，只是考虑了平面的情况。本博文提供一种简单有效的方法判断三维空间两线段是否相交，并提供完整matlab代码。

一、推导过程

设三维空间的线段 $A B$ 与 $C D$ 的端点分别为 $x_1,y_1,z_1), (x_2,y_2,z_2)$ 和 $x_3,y_3,z_3), (x_4,y_4,z_4)$ 。
线段 $A B$ 的参数方程为：
$\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}$
其中， $t\in[0,1]$ 。
线段 $C D$ 的参数方程为：
$\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}$
其中， $t'\in[0,1]$ 。
现在，两三维空间线段是否相交的问题转化为：是否存在 $t_1,t_2\in[0,1]$ , 使得下列方程组成立。
$\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}$
方程组(3)写成矩阵形式:
$\left[ \begin{matrix} t_1 \\ t_2 \\ \end{matrix} \tag 4 \right] =b$
其中， $\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$
1)当 $a^Ta$ 可逆(此时两线段存在交点或异面)时，利用最小二乘法求解方程组(4)得：
$\left[ \begin{matrix} t_1 \\ t_2 \\ \end{matrix} \tag 6 \right] =(a^Ta)^{-1}(a^Tb)$
进一步，可以写成：
$\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}$
其中， $A_{11} A_{22} - A_{12} A_{21}\ne0$ 。
解得 $t_1,t_2$ 后，判断其值是否在区间 $[0, 1]$ 内，若是，则进一步判断下式是否成立：
$\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}$
其中,eps为设定的容差。
若式(8)成立，则线段AB与线段CD相交，否则线段AB与线段CD异面。
2)当 $a^Ta$ 不可逆，也即 $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 < 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