空间解析几何2：空间中两线段/直线的距离【附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
结果验证：
下一章：空间解析几何-空间线段与平面的关系【附MATLAB代码】
原文地址：https://blog.csdn.net/y12345655/article/details/142172968
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！