求空间中两线段的最短距离 简单易懂 含matlab程序
目录
- 1 思路
- 2 matlab代码
1 思路
求空间两线段的最短距离和求空间两直线的最短距离不同，直线可以无限延伸，而线段不能。
设空间中有两条线段 AB和 CD，设A点的坐标为 ( x 1 , y 1 , z 1 ) (x_1,y_1,z_1) (x1,y1,z1)，B点的坐标为 ( x 2 , y 2 , z 2 ) (x_2,y_2,z_2) (x2,y2,z2), C点的坐标为 ( x 3 , y 3 , z 3 ) (x_3,y_3,z_3) (x3,y3,z3)，D点的坐标为 ( x 4 , y 4 , z 4 ) (x_4,y_4,z_4) (x4,y4,z4) 。
设P是直线AB上的一点，想象P点可以在直线AB上滑动，采用参数方程描述P点坐标。设P点坐标为 ( X , Y , Z ) (X,Y,Z) (X,Y,Z)，则：
{ X = x 1 + s ( x 2 − x 1 ) Y = y 1 + s ( y 2 − y 1 ) Z = z 1 + s ( z 2 − z 1 ) \left\{\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}\right. ⎩⎨⎧X=x1+s(x2−x1)Y=y1+s(y2−y1)Z=z1+s(z2−z1)
当 0 ≤ s ≤ 1 0 \leq s \leq 1 0≤s≤1时，P是线段AB上的点；当 s < 0 s<0 s<0时，P是BA延迟线上的点；当 s > 0 s>0 s>0时，P是AB延迟线上的点；
同理可以设Q是直线CD上的点，设Q点坐标为 ( U , V , W ) (U,V,W) (U,V,W)，则
{ U = x 3 + t ( x 4 − x 3 ) V = y 3 + t ( y 4 − y 3 ) W = z 3 + t ( z 4 − z 3 ) \left\{\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}\right. ⎩⎨⎧U=x3+t(x4−x3)V=y3+t(y4−y3)W=z3+t(z4−z3)
当 0 ≤ t ≤ 1 0 \leq t \leq 1 0≤t≤1时，Q是线段CD上的点；当 t < 0 t<0 t<0时，Q是DC延迟线上的点；当 t > 0 t>0 t>0时，Q是CD延迟线上的点；
那么P,Q之间的距离可以表示为：
P Q = ( X − U ) 2 + ( Y − V ) 2 + ( Z − W ) 2 P Q=\sqrt{(X-U)^{2}+(Y-V)^{2}+(Z-W)^{2}} PQ=(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)=P Q^{2}=(X-U)^{2}+(Y-V)^{2}+(Z-W)^{2} f(s,t)=PQ2=(X−U)2+(Y−V)2+(Z−W)2
这样就把问题转为成了求 f ( s , t ) f(s, t) f(s,t)最小值的问题，我们知道函数极值问题可以通过求导，令导数为0的方法解决，对多元函数也是一样。
对函数 f ( s , t ) f(s, t) f(s,t)求关于 s , t s,t s,t的偏导数，令偏导数为0，可以得到一个二元一次方程组。
{ ∂ f ( s , t ) ∂ s = 0 ∂ f ( s , t ) ∂ t = 0 \left\{\begin{array}{l} \frac{\partial f(s, t)}{\partial s}=0 \\ \frac{\partial f(s, t)}{\partial t}=0 \end{array}\right. {∂s∂f(s,t)=0∂t∂f(s,t)=0
求解这个方程得到 s , t s,t s,t的值，若 s , t s,t s,t满足， 0 ≤ s ≤ 1 0 \leq s \leq 1 0≤s≤1， 0 ≤ t ≤ 1 0 \leq t \leq 1 0≤t≤1,则P在线段AB上，Q在线段CD上。
若 s , t s,t s,t不满足， 0 ≤ s ≤ 1 0 \leq s \leq 1 0≤s≤1， 0 ≤ t ≤ 1 0 \leq t \leq 1 0≤t≤1时，举例说明。比如 s > 1 s>1 s>1，可以将问题转为求 s = 1 s=1 s=1时的P点到线段CD最短距离(这里大家想一想就明白了)。
求点到线段的最短距离原理和上文类似。还是设Q为直线CD上的点，Q点坐标为 ( U , V , W ) (U,V,W) (U,V,W)，则
{ U = x 3 + t ( x 4 − x 3 ) V = y 3 + t ( y 4 − y 3 ) W = z 3 + t ( z 4 − z 3 ) \left\{\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}\right. ⎩⎨⎧U=x3+t(x4−x3)V=y3+t(y4−y3)W=z3+t(z4−z3)
求点到线段的最短距离，还是求PQ的最短距离，但此时只有一个变量 t t t,我们可以令：
f ( t ) = P Q 2 = ( X − U ) 2 + ( Y − V ) 2 + ( Z − W ) 2 f(t)=P Q^{2}=(X-U)^{2}+(Y-V)^{2}+(Z-W)^{2} f(t)=PQ2=(X−U)2+(Y−V)2+(Z−W)2
对 f ( t ) f(t) f(t)关于 t t t求导，并令导数等于0，求出 t t t。然后对 t t t进行判断就可以了。若 t > 1 t>1 t>1，令 t = 1 t=1 t=1；若 t < 0 t<0 t<0，令 t = 0 t=0 t=0。最后我们将 s , t s,t s,t的值代入
P Q = ( X − U ) 2 + ( Y − V ) 2 + ( Z − W ) 2 P Q=\sqrt{(X-U)^{2}+(Y-V)^{2}+(Z-W)^{2}} PQ=(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