计算空间中两条直线之间最短距离 matlab

isDataWork

于 2023-07-20 19:28:31 发布

阅读量893

点赞数

文章标签： matlab 算法开发语言

本文链接：https://blog.csdn.net/qq_34995963/article/details/131827277

版权

参考链接：

求空间中两线段的最短距离简单易懂含matlab程序_天才小小傲的博客-CSDN博客

其实上面的博客已经实现了，不过使用的是matlab的求解器实现的，一般我们需要用C语言或者其他语言实现。因此本文做了一些改动。


close all
A=[0 4 0];
B=[0 1 0];
C=[3 0 -4];
D=[1 0 -4];
% PQ 为最短直线的两个端点
[P,Q]=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([P(1),Q(1)],[P(2),Q(2)],[P(3),Q(3)],'k--','LineWidth',1.5)

xlabel('X')
ylabel('Y')
zlabel('Z')
grid on
P
Q
plot3(P(1),P(3),P(3),'o');
plot3(Q(1),Q(2),Q(3),'*');

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

x1=A(1);
y1=A(2);
z1=A(3);

x2=B(1);
y2=B(2);
z2=B(3);

x3=C(1);
y3=C(2);
z3=C(3);

x4=D(1);
y4=D(2);
z4=D(3);

s =(x1^2*y3^2 - 2*x1^2*y3*y4 + x1^2*y4^2 + x1^2*z3^2 - 2*x1^2*z3*z4 + x1^2*z4^2 - 2*x1*x3*y1*y3 + 2*x1*x3*y1*y4 + x1*x3*y3*y4 + y2*x1*x3*y3 - x1*x3*y4^2 - y2*x1*x3*y4 - 2*x1*x3*z1*z3 + 2*x1*x3*z1*z4 + x1*x3*z3*z4 + z2*x1*x3*z3 - x1*x3*z4^2 - z2*x1*x3*z4 + 2*x1*x4*y1*y3 - 2*x1*x4*y1*y4 - x1*x4*y3^2 + x1*x4*y3*y4 - y2*x1*x4*y3 + y2*x1*x4*y4 + 2*x1*x4*z1*z3 - 2*x1*x4*z1*z4 - x1*x4*z3^2 + x1*x4*z3*z4 - z2*x1*x4*z3 + z2*x1*x4*z4 - x2*x1*y3^2 + 2*x2*x1*y3*y4 - x2*x1*y4^2 - x2*x1*z3^2 + 2*x2*x1*z3*z4 - x2*x1*z4^2 + x3^2*y1^2 - x3^2*y1*y4 - y2*x3^2*y1 + y2*x3^2*y4 + x3^2*z1^2 - x3^2*z1*z4 - z2*x3^2*z1 + z2*x3^2*z4 - 2*x3*x4*y1^2 + x3*x4*y1*y3 + x3*x4*y1*y4 + 2*y2*x3*x4*y1 - y2*x3*x4*y3 - y2*x3*x4*y4 - 2*x3*x4*z1^2 + x3*x4*z1*z3 + x3*x4*z1*z4 + 2*z2*x3*x4*z1 - z2*x3*x4*z3 - z2*x3*x4*z4 + x2*x3*y1*y3 - x2*x3*y1*y4 - x2*x3*y3*y4 + x2*x3*y4^2 + x2*x3*z1*z3 - x2*x3*z1*z4 - x2*x3*z3*z4 + x2*x3*z4^2 + x4^2*y1^2 - x4^2*y1*y3 - y2*x4^2*y1 + y2*x4^2*y3 + x4^2*z1^2 - x4^2*z1*z3 - z2*x4^2*z1 + z2*x4^2*z3 - x2*x4*y1*y3 + x2*x4*y1*y4 + x2*x4*y3^2 - x2*x4*y3*y4 - x2*x4*z1*z3 + x2*x4*z1*z4 + x2*x4*z3^2 - x2*x4*z3*z4 + y1^2*z3^2 - 2*y1^2*z3*z4 + y1^2*z4^2 - 2*y1*y3*z1*z3 + 2*y1*y3*z1*z4 + y1*y3*z3*z4 + z2*y1*y3*z3 - y1*y3*z4^2 - z2*y1*y3*z4 + 2*y1*y4*z1*z3 - 2*y1*y4*z1*z4 - y1*y4*z3^2 + y1*y4*z3*z4 - z2*y1*y4*z3 + z2*y1*y4*z4 - y2*y1*z3^2 + 2*y2*y1*z3*z4 - y2*y1*z4^2 + y3^2*z1^2 - y3^2*z1*z4 - z2*y3^2*z1 + z2*y3^2*z4 - 2*y3*y4*z1^2 + y3*y4*z1*z3 + y3*y4*z1*z4 + 2*z2*y3*y4*z1 - z2*y3*y4*z3 - z2*y3*y4*z4 + y2*y3*z1*z3 - y2*y3*z1*z4 - y2*y3*z3*z4 + y2*y3*z4^2 + y4^2*z1^2 - y4^2*z1*z3 - z2*y4^2*z1 + z2*y4^2*z3 - y2*y4*z1*z3 + y2*y4*z1*z4 + y2*y4*z3^2 - y2*y4*z3*z4)/(x1^2*y3^2 - 2*x1^2*y3*y4 + x1^2*y4^2 + x1^2*z3^2 - 2*x1^2*z3*z4 + x1^2*z4^2 - 2*x1*x2*y3^2 + 4*x1*x2*y3*y4 - 2*x1*x2*y4^2 - 2*x1*x2*z3^2 + 4*x1*x2*z3*z4 - 2*x1*x2*z4^2 - 2*x1*x3*y1*y3 + 2*x1*x3*y1*y4 + 2*x1*x3*y2*y3 - 2*x1*x3*y2*y4 - 2*x1*x3*z1*z3 + 2*x1*x3*z1*z4 + 2*x1*x3*z2*z3 - 2*x1*x3*z2*z4 + 2*x1*x4*y1*y3 - 2*x1*x4*y1*y4 - 2*x1*x4*y2*y3 + 2*x1*x4*y2*y4 + 2*x1*x4*z1*z3 - 2*x1*x4*z1*z4 - 2*x1*x4*z2*z3 + 2*x1*x4*z2*z4 + x2^2*y3^2 - 2*x2^2*y3*y4 + x2^2*y4^2 + x2^2*z3^2 - 2*x2^2*z3*z4 + x2^2*z4^2 + 2*x2*x3*y1*y3 - 2*x2*x3*y1*y4 - 2*x2*x3*y2*y3 + 2*x2*x3*y2*y4 + 2*x2*x3*z1*z3 - 2*x2*x3*z1*z4 - 2*x2*x3*z2*z3 + 2*x2*x3*z2*z4 - 2*x2*x4*y1*y3 + 2*x2*x4*y1*y4 + 2*x2*x4*y2*y3 - 2*x2*x4*y2*y4 - 2*x2*x4*z1*z3 + 2*x2*x4*z1*z4 + 2*x2*x4*z2*z3 - 2*x2*x4*z2*z4 + x3^2*y1^2 - 2*x3^2*y1*y2 + x3^2*y2^2 + x3^2*z1^2 - 2*x3^2*z1*z2 + x3^2*z2^2 - 2*x3*x4*y1^2 + 4*x3*x4*y1*y2 - 2*x3*x4*y2^2 - 2*x3*x4*z1^2 + 4*x3*x4*z1*z2 - 2*x3*x4*z2^2 + x4^2*y1^2 - 2*x4^2*y1*y2 + x4^2*y2^2 + x4^2*z1^2 - 2*x4^2*z1*z2 + x4^2*z2^2 + y1^2*z3^2 - 2*y1^2*z3*z4 + y1^2*z4^2 - 2*y1*y2*z3^2 + 4*y1*y2*z3*z4 - 2*y1*y2*z4^2 - 2*y1*y3*z1*z3 + 2*y1*y3*z1*z4 + 2*y1*y3*z2*z3 - 2*y1*y3*z2*z4 + 2*y1*y4*z1*z3 - 2*y1*y4*z1*z4 - 2*y1*y4*z2*z3 + 2*y1*y4*z2*z4 + y2^2*z3^2 - 2*y2^2*z3*z4 + y2^2*z4^2 + 2*y2*y3*z1*z3 - 2*y2*y3*z1*z4 - 2*y2*y3*z2*z3 + 2*y2*y3*z2*z4 - 2*y2*y4*z1*z3 + 2*y2*y4*z1*z4 + 2*y2*y4*z2*z3 - 2*y2*y4*z2*z4 + y3^2*z1^2 - 2*y3^2*z1*z2 + y3^2*z2^2 - 2*y3*y4*z1^2 + 4*y3*y4*z1*z2 - 2*y3*y4*z2^2 + y4^2*z1^2 - 2*y4^2*z1*z2 + y4^2*z2^2);
t =(- x1^2*y2*y3 + y4*x1^2*y2 + x1^2*y3^2 - y4*x1^2*y3 - x1^2*z2*z3 + z4*x1^2*z2 + x1^2*z3^2 - z4*x1^2*z3 + x1*x2*y1*y3 - y4*x1*x2*y1 + x1*x2*y2*y3 - y4*x1*x2*y2 - 2*x1*x2*y3^2 + 2*y4*x1*x2*y3 + x1*x2*z1*z3 - z4*x1*x2*z1 + x1*x2*z2*z3 - z4*x1*x2*z2 - 2*x1*x2*z3^2 + 2*z4*x1*x2*z3 + x1*x3*y1*y2 - 2*x1*x3*y1*y3 + y4*x1*x3*y1 - x1*x3*y2^2 + 2*x1*x3*y2*y3 - y4*x1*x3*y2 + x1*x3*z1*z2 - 2*x1*x3*z1*z3 + z4*x1*x3*z1 - x1*x3*z2^2 + 2*x1*x3*z2*z3 - z4*x1*x3*z2 - x4*x1*y1*y2 + x4*x1*y1*y3 + x4*x1*y2^2 - x4*x1*y2*y3 - x4*x1*z1*z2 + x4*x1*z1*z3 + x4*x1*z2^2 - x4*x1*z2*z3 - x2^2*y1*y3 + y4*x2^2*y1 + x2^2*y3^2 - y4*x2^2*y3 - x2^2*z1*z3 + z4*x2^2*z1 + x2^2*z3^2 - z4*x2^2*z3 - x2*x3*y1^2 + x2*x3*y1*y2 + 2*x2*x3*y1*y3 - y4*x2*x3*y1 - 2*x2*x3*y2*y3 + y4*x2*x3*y2 - x2*x3*z1^2 + x2*x3*z1*z2 + 2*x2*x3*z1*z3 - z4*x2*x3*z1 - 2*x2*x3*z2*z3 + z4*x2*x3*z2 + x4*x2*y1^2 - x4*x2*y1*y2 - x4*x2*y1*y3 + x4*x2*y2*y3 + x4*x2*z1^2 - x4*x2*z1*z2 - x4*x2*z1*z3 + x4*x2*z2*z3 + x3^2*y1^2 - 2*x3^2*y1*y2 + x3^2*y2^2 + x3^2*z1^2 - 2*x3^2*z1*z2 + x3^2*z2^2 - x4*x3*y1^2 + 2*x4*x3*y1*y2 - x4*x3*y2^2 - x4*x3*z1^2 + 2*x4*x3*z1*z2 - x4*x3*z2^2 - y1^2*z2*z3 + z4*y1^2*z2 + y1^2*z3^2 - z4*y1^2*z3 + y1*y2*z1*z3 - z4*y1*y2*z1 + y1*y2*z2*z3 - z4*y1*y2*z2 - 2*y1*y2*z3^2 + 2*z4*y1*y2*z3 + y1*y3*z1*z2 - 2*y1*y3*z1*z3 + z4*y1*y3*z1 - y1*y3*z2^2 + 2*y1*y3*z2*z3 - z4*y1*y3*z2 - y4*y1*z1*z2 + y4*y1*z1*z3 + y4*y1*z2^2 - y4*y1*z2*z3 - y2^2*z1*z3 + z4*y2^2*z1 + y2^2*z3^2 - z4*y2^2*z3 - y2*y3*z1^2 + y2*y3*z1*z2 + 2*y2*y3*z1*z3 - z4*y2*y3*z1 - 2*y2*y3*z2*z3 + z4*y2*y3*z2 + y4*y2*z1^2 - y4*y2*z1*z2 - y4*y2*z1*z3 + y4*y2*z2*z3 + y3^2*z1^2 - 2*y3^2*z1*z2 + y3^2*z2^2 - y4*y3*z1^2 + 2*y4*y3*z1*z2 - y4*y3*z2^2)/(x1^2*y3^2 - 2*x1^2*y3*y4 + x1^2*y4^2 + x1^2*z3^2 - 2*x1^2*z3*z4 + x1^2*z4^2 - 2*x1*x2*y3^2 + 4*x1*x2*y3*y4 - 2*x1*x2*y4^2 - 2*x1*x2*z3^2 + 4*x1*x2*z3*z4 - 2*x1*x2*z4^2 - 2*x1*x3*y1*y3 + 2*x1*x3*y1*y4 + 2*x1*x3*y2*y3 - 2*x1*x3*y2*y4 - 2*x1*x3*z1*z3 + 2*x1*x3*z1*z4 + 2*x1*x3*z2*z3 - 2*x1*x3*z2*z4 + 2*x1*x4*y1*y3 - 2*x1*x4*y1*y4 - 2*x1*x4*y2*y3 + 2*x1*x4*y2*y4 + 2*x1*x4*z1*z3 - 2*x1*x4*z1*z4 - 2*x1*x4*z2*z3 + 2*x1*x4*z2*z4 + x2^2*y3^2 - 2*x2^2*y3*y4 + x2^2*y4^2 + x2^2*z3^2 - 2*x2^2*z3*z4 + x2^2*z4^2 + 2*x2*x3*y1*y3 - 2*x2*x3*y1*y4 - 2*x2*x3*y2*y3 + 2*x2*x3*y2*y4 + 2*x2*x3*z1*z3 - 2*x2*x3*z1*z4 - 2*x2*x3*z2*z3 + 2*x2*x3*z2*z4 - 2*x2*x4*y1*y3 + 2*x2*x4*y1*y4 + 2*x2*x4*y2*y3 - 2*x2*x4*y2*y4 - 2*x2*x4*z1*z3 + 2*x2*x4*z1*z4 + 2*x2*x4*z2*z3 - 2*x2*x4*z2*z4 + x3^2*y1^2 - 2*x3^2*y1*y2 + x3^2*y2^2 + x3^2*z1^2 - 2*x3^2*z1*z2 + x3^2*z2^2 - 2*x3*x4*y1^2 + 4*x3*x4*y1*y2 - 2*x3*x4*y2^2 - 2*x3*x4*z1^2 + 4*x3*x4*z1*z2 - 2*x3*x4*z2^2 + x4^2*y1^2 - 2*x4^2*y1*y2 + x4^2*y2^2 + x4^2*z1^2 - 2*x4^2*z1*z2 + x4^2*z2^2 + y1^2*z3^2 - 2*y1^2*z3*z4 + y1^2*z4^2 - 2*y1*y2*z3^2 + 4*y1*y2*z3*z4 - 2*y1*y2*z4^2 - 2*y1*y3*z1*z3 + 2*y1*y3*z1*z4 + 2*y1*y3*z2*z3 - 2*y1*y3*z2*z4 + 2*y1*y4*z1*z3 - 2*y1*y4*z1*z4 - 2*y1*y4*z2*z3 + 2*y1*y4*z2*z4 + y2^2*z3^2 - 2*y2^2*z3*z4 + y2^2*z4^2 + 2*y2*y3*z1*z3 - 2*y2*y3*z1*z4 - 2*y2*y3*z2*z3 + 2*y2*y3*z2*z4 - 2*y2*y4*z1*z3 + 2*y2*y4*z1*z4 + 2*y2*y4*z2*z3 - 2*y2*y4*z2*z4 + y3^2*z1^2 - 2*y3^2*z1*z2 + y3^2*z2^2 - 2*y3*y4*z1^2 + 4*y3*y4*z1*z2 - 2*y3*y4*z2^2 + y4^2*z1^2 - 2*y4^2*z1*z2 + y4^2*z2^2);
% 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);
% %解二元一次方程
% [s1,t1]=solve(eqns,s,t)l



X=x1+s*(x2-x1);
Y=y1+s*(y2-y1);
Z=z1+s*(z2-z1);

U=x3+t*(x4-x3);
V=y3+t*(y4-y3);
W=z3+t*(z4-z3);

P=[X,Y,Z];
Q=[U,V,W];
end

符号求解部分

% 符号求解
syms s t x1 y1 z1 x2 y2 z2 x3 y3 z3 x4 y4 z4

X=x1+s*(x2-x1);
Y=y1+s*(y2-y1);
Z=z1+s*(z2-z1);
P=[X,Y,Z];
U=x3+t*(x4-x3);
V=y3+t*(y4-y3);
W=z3+t*(z4-z3);
Q=[U,V,W];
Fst=sum((P-Q).^2);


eqns(1)=(diff(Fst,s)==0);
eqns(2)=(diff(Fst,t)==0);
%解二元一次方程
[se,te]=solve(eqns,s,t)

t=0;
s=0;
s=subs(se)
t=subs(te)