【Matlab案例】求空间中两线段的最短距离 简单易懂 含matlab程序

1 思路

  求空间两线段的最短距离和求空间两直线的最短距离不同，直线可以无限延伸，而线段不能。
  设空间中有两条线段 AB和 CD，设A点的坐标为 $(x_1,y_1,z_1)$，B点的坐标为 $(x_2,y_2,z_2)$, C点的坐标为 $(x_3,y_3,z_3)$，D点的坐标为$(x_4,y_4,z_4)$ 。

  设P是直线AB上的一点，想象P点可以在直线AB上滑动，采用参数方程描述P点坐标。设P点坐标为 $(X,Y,Z)$，则：
$$\{\begin{array}{l}X=x_1+s\left(x_2-x_1\right)\\ Y=y_1+s\left(y_2-y_1\right)\\ Z=z_1+s\left(z_2-z_1\right)\end{array}$$
  当$0\le s\le 1$时，P是线段AB上的点；当$s<0$时，P是BA延迟线上的点；当$s>0$时，P是AB延迟线上的点；

  同理可以设Q是直线CD上的点，设Q点坐标为 $(U,V,W)$，则
$$\{\begin{array}{l}U=x_3+t\left(x_4-x_3\right)\\ V=y_3+t\left(y_4-y_3\right)\\ W=z_3+t\left(z_4-z_3\right)\end{array}$$
  当$0\le t\le 1$时，Q是线段CD上的点；当$t<0$时，Q是DC延迟线上的点；当$t>0$时，Q是CD延迟线上的点；

  那么P,Q之间的距离可以表示为：
$$PQ=\sqrt{(X-U{)}^2+(Y-V{)}^2+(Z-W{)}^2}$$

求两直线的最短距离，就是求PQ的最短距离。我们可以令：
$$f(s,t)=P{Q}^2=(X-U{)}^2+(Y-V{)}^2+(Z-W{)}^2$$
  这样就把问题转为成了求$f(s,t)$最小值的问题，我们知道函数极值问题可以通过求导，令导数为0的方法解决，对多元函数也是一样。
  对函数$f(s,t)$求关于$s,t$的偏导数，令偏导数为0，可以得到一个二元一次方程组。
$$\{\begin{array}{l}\frac{\partial f(s,t)}{\partial s}=0\\ \frac{\partial f(s,t)}{\partial t}=0\end{array}$$
  求解这个方程得到$s,t$的值，若$s,t$满足，$0\le s\le 1$，$0\le t\le 1$,则P在线段AB上，Q在线段CD上。

  若$s,t$不满足，$0\le s\le 1$，$0\le t\le 1$时，举例说明。比如$s>1$，可以将问题转为求$s=1$时的P点到线段CD最短距离(这里大家想一想就明白了)。
  求点到线段的最短距离原理和上文类似。还是设Q为直线CD上的点，Q点坐标为 $(U,V,W)$，则
$$\{\begin{array}{l}U=x_3+t\left(x_4-x_3\right)\\ V=y_3+t\left(y_4-y_3\right)\\ W=z_3+t\left(z_4-z_3\right)\end{array}$$
  求点到线段的最短距离，还是求PQ的最短距离，但此时只有一个变量$t$,我们可以令：
$$f(t)=P{Q}^2=(X-U{)}^2+(Y-V{)}^2+(Z-W{)}^2$$
  对$f(t)$关于$t$求导，并令导数等于0，求出$t$。然后对$t$进行判断就可以了。若$t>1$，令$t=1$；若$t<0$，令$t=0$。最后我们将$s,t$的值代入
$$PQ=\sqrt{(X-U{)}^2+(Y-V{)}^2+(Z-W{)}^2}$$
就完事了！

2 matlab代码

对大家有帮助的话记得点个赞，收藏一下！！

主函数

%main.m
A=[0 3 0];
B=[3 3 0];
C=[2 5 2];
D=[5 5 4];
%dis为线段AB、CD最短距离 
[dis,P1,Q1,P2,Q2]=segmentDistance(A,B,C,D);
%画线段AB
plot3([A(1),B(1)],[A(2),B(2)],[A(3),B(3)],'r','LineWidth',2)
hold on
%画线段CD
plot3([C(1),D(1)],[C(2),D(2)],[C(3),D(3)],'b','LineWidth',2)
hold on
%画直线AB、CD最短距离 
plot3([P1(1),Q1(1)],[P1(2),Q1(2)],[P1(3),Q1(3)],'k--','LineWidth',1.5)
hold on
%画线段AB、CD最短距离 
plot3([P2(1),Q2(1)],[P2(2),Q2(2)],[P2(3),Q2(3)],'c','LineWidth',1)
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on

两线段最短距离函数

function [dis,P1,Q1,P2,Q2]=segmentDistance(A,B,C,D)
%求两线段之间的最短距离
%[dis,P1,Q1,P2,Q2]=segmentDistance(A,B,C,D)
%A,B为第一条线段的首末端点 C,D为第二条线段的首末短线
%dis为最短距离 P2 Q2为最短距离的首末端点
%P1,Q1为直线AB、CD的最短距离首末端点
%P2,Q2为线段AB、CD的最短距离首末端点

syms s t
%s取0~1时P点在线段AB上滑动，s>1 P点在B端点外，s<1 P点在A端点外
P=A+s.*(B-A);
%t取0~1时Q点在线段CD上滑动，t>1 Q点在D端点外，s<1 P点在C端点外
Q=C+t.*(D-C);
%求P点到Q点的距离的平方
Fst=sum((P-Q).^2);
%求偏导
eqns(1)=(diff(Fst,s)==0);
eqns(2)=(diff(Fst,t)==0);
%解二元一次方程
[s,t]=solve(eqns,s,t);
P1=subs(P);
Q1=subs(Q);
%如果两线段最短距离不在线段上 进行判断
if s>1
    s=1;
    [dis,P2,Q2]=pointSegmentDistance(C,D,subs(P));
elseif s<0
    s=0;
    [dis,P2,Q2]=pointSegmentDistance(C,D,subs(P));
elseif t>1
    t=1;
    [dis,P2,Q2]=pointSegmentDistance(A,B,subs(Q));
elseif t<0
    t=0;
    [dis,P2,Q2]=pointSegmentDistance(A,B,subs(Q));
else
    dis=norm(subs(P)-subs(Q));
    P2=P1;
    Q2=Q1;
end
end

点到线段最短距离函数

function [dis,P2,Q2]=pointSegmentDistance(A,B,C)
%求空间一点到一线段的最短距离 
%[dis,P2,Q2]=pointSegmentDistance(A,B,C)
%A B为线段首末端点，C为空间一点
%dis为最短距离 P2 Q2为最短距离的首末端点

syms s
%s取0~1时P点在线段AB上滑动，s>1 P点在B端点外，s<1 P点在A端点外
P=A+s.*(B-A);
Fs=sum((P-C).^2);
eqns=(diff(Fs,s)==0);
s=solve(eqns,s);
if s>1
    s=1;
    dis=norm(subs(P)-C);
elseif s<0
    s=0;
    dis=norm(subs(P)-C);
else
    dis=norm(subs(P)-C);
end
P2=subs(P);
Q2=C;
end