目录

理论公式

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]);
        end
        end
    end
    dmin = 1000000000;
    roots = [root,y,i];
    for j=1:i
        theta = 2*atan(root(j));
        t = -(a21*cos(theta)+a22*sin(theta)+a24)/a23;
        if(t>l)
            t=l;
        end
        if(t<0)
            t = 0;
        end
        Rx = 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];
        end
    end   
    %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;
    end
    alpha=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);
    else
        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);
    end
    root4{j}=[root21,root22];
    i4{j}=[i21,i22];
    root=[root,root4{j}];
    i=i+i21+i22;
    break

end
for i_index=length(root):-1:1
    for j=i_index-1:-1:1
        if(abs(root(i_index)-root(j))<0.00001)
            root=root(1:length(root)-1);
            i=i-1;
            break;
        end
    end
end
y=root.^4+a*root.^3+b*root.^2+c*root+d;
end

function [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;
else
    i=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;
end

function [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;
else
    i=0;
    root=[];
end
y=a*root.^2+b*root+c;
end

下一章：空间解析几何5-空间圆到平面的距离【附MATLAB代码】