目录
理论公式
对于解一元4次方程，请详见我的博客
matlab代码
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代码】