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

本文链接：https://blog.youkuaiyun.com/y12345655/article/details/143077102

理论公式

matlab代码

function [dis,P,Q,L]=Circle2PlaneDistance(T,R,n,Pn)
% output
% dis 为最短距离，P为距离最短时圆上的点 Q为P对应的投影点 L为最小值有几个
% input
% T为园心到基坐标系的变换矩阵 R为圆半径 n为平面的单位法向量，Pn为平面上一点
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;
    end
    for t=1:i
    b = sqrt((A*cos(theta(t))+B*sin(theta(t))+C)^2);
    if(b<d)
        d=b;
        alf = theta(t);
    end
     if(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;
     end
    end
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;
    end
    for t=1:i
    b = sqrt((A*cos(theta(i))+B*sin(theta(i))+C)^2);
    if(b<d)
        d=b;
        alf = theta(t);
    end
    if(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;
    end
    end
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]);
    end
    end
    end
    for t=1:i
    theta = 2*atan(root(t));
    b = sqrt((A*cos(theta)+B*sin(theta)+C)^2);
    if(b<d)
        d=b;
        alf = theta;
    end
    if(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;
    end
    end
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
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

测试代码：

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)

测试结果：