目錄

理論公式

matlab代碼

理論公式

matlab代碼

function [dis,P,Q,L]=Circle2PlaneDistance(T,R,n,Pn)
% output
% dis 為最短距離，P為距離最短時(shí)圓上的點(diǎn) Q為P對(duì)應(yīng)的投影點(diǎn) L為最小值有幾個(gè)
% input
% T為園心到基坐標(biāo)系的變換矩陣 R為圓半徑 n為平面的單位法向量，Pn為平面上一點(diǎn)
d = 1000;
alf = 0;
sym i;
y=0;
P=[];
Q=[];
L = 0;
A = R*(n(1)*T(1,1)+n(2)*T(2,1)+n(3)*T(3,1));
B = R*(n(1)*T(1,2)+n(2)*T(2,2)+n(3)*T(3,2));
C = n(1)*T(1,4)+n(2)*T(2,4)+n(3)*T(3,4)-(n(1)*Pn(1)+n(2)*Pn(2)+n(3)*Pn(3));
if(B==0)theta(1) = 0;theta(2) = pi;i=2;if(-1<=C&&C<=1)theta(3) = acos(-C);i=3;endfor t=1:ib = sqrt((A*cos(theta(t))+B*sin(theta(t))+C)^2);if(b<d)d=b;alf = theta(t);endif(abs(b) < 1e-8)alf = theta;P = [R*T(1,1)*cos(alf)+R*T(1,2)*sin(alf)+T(1,4),R*T(2,1)*cos(alf)+R*T(2,2)*sin(alf)+T(2,4),R*T(3,1)*cos(alf)+R*T(3,2)*sin(alf)+T(3,4)];tt =  n(1)*P(1)+n(2)*P(2)+n(3)*P(3)-(n(1)*Pn(1)+n(2)*Pn(2)+n(3)*Pn(3));Q = [Q;P(1)-n(1)*tt,P(2)-n(2)*tt,P(3)-n(3)*tt];L=L+1;endend
end
if(A==0)theta(1) = pi/2;theta(2) = -pi/2;i=2;if(-1<=C&&C<=1)theta(3) = asin(-C);i=3;endfor t=1:ib = sqrt((A*cos(theta(i))+B*sin(theta(i))+C)^2);if(b<d)d=b;alf = theta(t);endif(abs(b) < 1e-8)alf = theta;P = [R*T(1,1)*cos(alf)+R*T(1,2)*sin(alf)+T(1,4),R*T(2,1)*cos(alf)+R*T(2,2)*sin(alf)+T(2,4),R*T(3,1)*cos(alf)+R*T(3,2)*sin(alf)+T(3,4)];tt =  n(1)*P(1)+n(2)*P(2)+n(3)*P(3)-(n(1)*Pn(1)+n(2)*Pn(2)+n(3)*Pn(3));Q = [Q;P(1)-n(1)*tt,P(2)-n(2)*tt,P(3)-n(3)*tt];L=L+1;endend
end
u = A*B-B*C;
v = 2*A*A-2*B*B-2*A*C;
w = -6*A*B;
g = 2*B*B-2*A*A-2*A*C;
h = A*B+B*C;
[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([v,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]);endendendfor t=1:itheta = 2*atan(root(t));b = sqrt((A*cos(theta)+B*sin(theta)+C)^2);if(b<d)d=b;alf = theta;endif(abs(b) < 1e-8)alf = theta;P = [R*T(1,1)*cos(alf)+R*T(1,2)*sin(alf)+T(1,4),R*T(2,1)*cos(alf)+R*T(2,2)*sin(alf)+T(2,4),R*T(3,1)*cos(alf)+R*T(3,2)*sin(alf)+T(3,4)];tt =  n(1)*P(1)+n(2)*P(2)+n(3)*P(3)-(n(1)*Pn(1)+n(2)*Pn(2)+n(3)*Pn(3));Q = [Q;P(1)-n(1)*tt,P(2)-n(2)*tt,P(3)-n(3)*tt];L=L+1;endend
TB = alf*180/pi;
dis = d;
if(abs(dis) > 1e-8)P = [R*T(1,1)*cos(alf)+R*T(1,2)*sin(alf)+T(1,4),R*T(2,1)*cos(alf)+R*T(2,2)*sin(alf)+T(2,4),R*T(3,1)*cos(alf)+R*T(3,2)*sin(alf)+T(3,4)];tt =  n(1)*P(1)+n(2)*P(2)+n(3)*P(3)-(n(1)*Pn(1)+n(2)*Pn(2)+n(3)*Pn(3));Q = [P(1)-n(1)*tt,P(2)-n(2)*tt,P(3)-n(3)*tt];L=L+1;
end
endfunction [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;break
end
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;
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;
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

測(cè)試代碼：

Tc1 = [  -0.5662    0.7741    0.2831    1.0000;-0.6924   -0.6330    0.3462    1.0000;0.4472         0    0.8944    1.0000;0         0         0    1.0000];
R = 3;
n = [0 0 1];
pn = [0 0 0];
[dis,P,Q] = Circle2PlaneDistance(Tc1,R,n,Pn)

測(cè)試結(jié)果：