当前位置：首页 > news >正文

空间解析几何 4：空间中线段到圆的距离【附MATLAB代码】

news 2024/10/22 17:19:59

理论公式

matlab代码

理论公式

对于解一元4次方程，请详见我的博客

一元四次方程求解 -【附MATLAB代码】-CSDN博客文章浏览阅读1.4k次，点赞41次，收藏4次。最近在研究机器人的干涉（碰撞）检测，遇到了一个问题，就是在求椭圆到原点的最短距离时，构建的方程是一个一元四次方程。无论是高中的初等数学，大学的高等数学，还是研究生的高等代数，都没有关于一元四次方程的求解方法，大多都是一元二次方程的求解。仔细一研究才知道为什么很少提及一元四次方程。_一元四次方程https://blog.csdn.net/y12345655/article/details/141368800?spm=1001.2014.3001.5502

matlab代码

function [dmin ,P,Q] = circle_line(R,T,P1,P2)
dmin = 10000;
%[dminAD,thetaAD,deltaAD] = AD_circle_line(R,T,P',Q');A = P2(1)-P1(1);B = P2(2)-P1(2);C = P2(3)-P1(3);lemda = P2-P1;lemda=lemda/norm(lemda);A=lemda(1);B=lemda(2);C=lemda(3);l = sqrt((P2(1)- P1(1))^2+(P2(2)- P1(2))^2+(P2(3)- P1(3))^2);D = T(1,4)-P1(1);E = T(2,4)-P1(2);F = T(3,4)-P1(3);a11 = R*(D*T(1,2)+E*T(2,2)+F*T(3,2));a12 = -R*(D*T(1,1)+E*T(2,1)+F*T(3,1));a13 = -R*(A*T(1,2)+B*T(2,2)+C*T(3,2));a14 =  R*(A*T(1,1)+B*T(2,1)+C*T(3,1));a21 = -a14;a22 = a13;a23 = A*A+B*B+C*C;a24 = -(A*D+B*E+C*F);%   PPYS = [a11,a12,a13,a14,a21,a22,a23,a24]AA = a13*a14;BB = (a14*a14-a13*a13)/2;CC = a11*a23-a13*a24;DD = a12*a23-a14*a24;test=[AA BB CC DD];u = AA-CC;v = 2*DD-4*BB;w= -6*AA;g = 4*BB+2*DD;h = AA+CC;[u,v,w,g,h];if(u == 0&&v==0&&w==0)root = 0;i = 1;else if(u == 0&&v==0)[root,y,i]= Solve2OrderEquaton([w,g,h]);else if(u == 0)[root,y,i]= Solve3OrderEquaton([v,w,g,h]);else[root,y,i]= Solve4OrderEquaton([u,v,w,g,h]);endendenddmin = 1000000000;roots = [root,y,i];for j=1:itheta = 2*atan(root(j));t = -(a21*cos(theta)+a22*sin(theta)+a24)/a23;if(t>l)t=l;endif(t<0)t = 0;endRx = R*T(1,1)*cos(theta)+R*T(1,2)*sin(theta)+T(1,4);Ry = R*T(2,1)*cos(theta)+R*T(2,2)*sin(theta)+T(2,4);Rz = R*T(3,1)*cos(theta)+R*T(3,2)*sin(theta)+T(3,4);Px = P1(1)+A*t;Py = P1(2)+B*t;Pz = P1(3)+C*t;d = sqrt((Rx-Px)^2+(Ry-Py)^2+(Rz-Pz)^2);if(dmin>d)alf = theta;dmin = d;ttas = t;P = [Rx,Ry,Rz];Q = [Px,Py,Pz];endend   %alf*180/pi;
end
function [root,y,i] = Solve4OrderEquaton(parameter)
a=parameter(2)/parameter(1);
b=parameter(3)/parameter(1);
c=parameter(4)/parameter(1);
d=parameter(5)/parameter(1);a3=1;
b3=-b;
c3=(a*c-4*d);
d3=-(a^2*d-4*b*d+c^2);
parameter3=[a3,b3,c3,d3];
[root3,y3,i3] = Solve3OrderEquaton(parameter3);
i=0;
root=[];
for j=1:length(root3)if(a^2/4-b+root3(j)<0||root3(j)^2/4-d<0)continue;endalpha=sqrt(a^2/4-b+root3(j));beta=sqrt(root3(j)^2/4-d);if(a*root3(j)/2-c>0)a21=1;b21=a/2-alpha;c21=root3(j)/2-beta;parameter21=[a21,b21,c21];[root21,y21,i21] = Solve2OrderEquaton(parameter21);a22=1;b22=a/2+alpha;c22=root3(j)/2+beta;parameter22=[a22,b22,c22];[root22,y22,i22] = Solve2OrderEquaton(parameter22);elsea21=1;b21=a/2-alpha;c21=root3(j)/2+beta;parameter21=[a21,b21,c21];[root21,y21,i21] = Solve2OrderEquaton(parameter21);a22=1;b22=a/2+alpha;c22=root3(j)/2-beta;parameter22=[a22,b22,c22];[root22,y22,i22] = Solve2OrderEquaton(parameter22);endroot4{j}=[root21,root22];i4{j}=[i21,i22];root=[root,root4{j}];i=i+i21+i22;breakend
for i_index=length(root):-1:1for j=i_index-1:-1:1if(abs(root(i_index)-root(j))<0.00001)root=root(1:length(root)-1);i=i-1;break;endend
end
y=root.^4+a*root.^3+b*root.^2+c*root+d;
endfunction [root,y,i] = Solve3OrderEquaton(parameter)
a=parameter(1);
b=parameter(2);
c=parameter(3);
d=parameter(4);
a_2=a*a;
a_3=a_2*a;
b_2=b*b;
b_3=b_2*b;
p=c/3/a-b_2/9/a_2;
q=d/2/a+b_3/27/a_3-b*c/6/a_2;
delta=q*q+p^3;
if(delta>0)i=1;root=nthroot(-q+sqrt(delta),3)+nthroot(-q-sqrt(delta),3)-b/3/a;
elseif(delta==0)i=2;root(1)=-2*nthroot(q,3)-b/3/a;root(2)=nthroot(q,3)-b/3/a;
elsei=3;alpha=1/3*acos(-q*sqrt(-p)/p^2);root(1)=2*sqrt(-p)*cos(alpha)-b/3/a;root(2)=2*sqrt(-p)*cos(alpha+2/3*pi)-b/3/a;root(3)=2*sqrt(-p)*cos(alpha+4/3*pi)-b/3/a;
end
y=a*root.^3+b*root.^2+c*root+d;
endfunction [root,y,i] = Solve2OrderEquaton(parameter)
a=parameter(1);
b=parameter(2);
c=parameter(3);
delta=b^2-4*a*c;
if(delta>0)i=2;root(1)=(-b+sqrt(delta))/2/a;root(2)=(-b-sqrt(delta))/2/a;
elseif(delta==0)i=1;root=-b/2/a;
elsei=0;root=[];
end
y=a*root.^2+b*root+c;
end

下一章：空间解析几何5-空间圆到平面的距离【附MATLAB代码】https://blog.csdn.net/y12345655/article/details/143077102?spm=1001.2014.3001.5502