空间解析几何2：空间中两线段/直线的距离【附MATLAB代码】

理论公式

MATLAB代码

理论公式

MATLAB代码

公式实现

function [dis,P,Q,t1,s1]=line2LineDistance(A1,B1,C1,D1)
%求两线段的最短距离
% input
% A1,B1为线段一的两端点 C1,D1为线段二的两端点
% output
% dis,为两线段的最短距离，P,Q为距离最短时在两线段上的坐标
% t1 为第一条直线的参数t[0,1] s1 为第二条直线的参数t[0,1]
dis=0;
P=[];
Q=[];A = B1(1)-A1(1);
B = B1(2)-A1(2);
C = B1(3)-A1(3);D = D1(1)-C1(1);
E = D1(2)-C1(2);
F = D1(3)-C1(3);G = A1(1)*D+ A1(2)*E+ A1(3)*F-C1(1)*D-C1(2)*E-C1(3)*F;
H = A*D +B*E+C*F;
I = D*D+E*E+F*F;
J = A*A+B*B+C*C;
K = A1(1)*A+ A1(2)*B+ A1(3)*C-C1(1)*A-C1(2)*B-C1(3)*C;
[G H I J K];
if(H*H==I*J)s=0.5;t= (s*H-K)/J;
else
s = (H*K-G*J)/(H*H-I*J);
t = (s*H-K)/J;%dis = sqrt((x1-x2)^2+(y1-y2)^2+(z1-z2)^2)if s>1s=1;x2 = C1(1) + D*s;y2 = C1(2) + E*s;z2 = C1(3) + F*s;Q = [x2 y2 z2];[dis,P,t]=point2Line(A1,B1,Q);
elseif s<0s=0;x2 = C1(1) + D*s;y2 = C1(2) + E*s;z2 = C1(3) + F*s;Q = [x2 y2 z2];[dis,P,t]=point2Line(A1,B1,Q);
elseif t>1t=1;x1 = A1(1) + A*t;y1 = A1(2) + B*t;z1 = A1(3) + C*t;P = [x1 y1 z1];[dis,Q,s]=point2Line(C1,D1,P);
elseif t<0t=0;x1 = A1(1) + A*t;y1 = A1(2) + B*t;z1 = A1(3) + C*t;P = [x1 y1 z1];[dis,Q,s]=point2Line(C1,D1,P);
elsex1 = A1(1) + A*t;y1 = A1(2) + B*t;z1 = A1(3) + C*t;x2 = C1(1) + D*s;y2 = C1(2) + E*s;z2 = C1(3) + F*s;P = [x1 y1 z1];Q = [x2 y2 z2];dis = sqrt((x1-x2)^2+(y1-y2)^2+(z1-z2)^2);
end
t1 = t;
s1 = s;
end
end
function [dis,P2,t]= point2Line (A1,B1,C1)
%求空间一点到一线段的最短距离 
%[dis,P2,Q2]=pointSegmentDistance(A,B,C)
%A B为线段首末端点，C为空间一点
%dis为最短距离 P2 Q2为最短距离的首末端点% 取0~1时P点在线段AB上滑动，>1 P点在B端点外，<0 P点在A端点外
A = B1(1)-A1(1);
B = B1(2)-A1(2);
C = B1(3)-A1(3);
D = A1(1)-C1(1);
E = A1(2)-C1(2);
F = A1(3)-C1(3);
t = -(A*D+B*E+C*F)/(A*A+B*B+C*C);
if t>1t=1;
elseif t<0t=0;
end
x1 = A1(1) + A*t;
y1 = A1(2) + B*t;
z1 = A1(3) + C*t;
dis = sqrt((x1-C1(1))^2+(y1-C1(2))^2+(z1-C1(3))^2);
P2 = [x1 ,y1,z1];
end

测试代码

A = [-229.719,419.403,-200.000];
B = [-229.719,453.815,-549.490];
C=[-298.4758,  401.6892, -327.4938];
D=[182.5318,  339.5882, -327.8367];
%dis为线段AB、CD最短距离 
[dis,P,Q]=myDistance(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([P(1),Q(1)],[P(2),Q(2)],[P(3),Q(3)],'k--','LineWidth',1.5)
hold on
%画线段AB、CD最短距离 
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on

结果验证：