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

news2024/9/21 22:24:00

理论公式

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>1
    s=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<0
    s=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>1
    t=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<0
    t=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);
else
    x1 = 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>1
    t=1;
elseif t<0
    t=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

结果验证：