空间解析几何7：空间圆与矩形的最短距离【附MATLAB代码】

目录

理论推导

matlab代码

理论推导

matlab代码

function [dmin] = YS_distanceCircleToPlane(length,width,R,Tc1)
% input
% length  矩形的长
% width   矩形的宽
% R 圆的半径
% Tc1 圆心基于矩形第三象限为原点的坐标变换
% output
% dis 最短距离
dmin = 10000;
P1 = [length,width,0];
P2 = [length,0,0];
P3 = [0,0,0];
P4 = [0,width,0];
rect = [length,width,0;
        length,0,0;
        0,0,0;
        0,width,0
        ];
% 先考虑平行情况
if(abs(abs(Tc1(3,3))-1)<1e-8)
    dmin = abs(Tc1(3,4));
    return;
end
a = P2(1)-P3(1);
b = P4(2)-P3(2);
c = P3(1)- Tc1(1,4);
d = P3(2)- Tc1(2,4);
e = -Tc1(3,4);
A = R*(Tc1(1,1)^2+Tc1(2,1)^2-Tc1(1,2)^2-Tc1(2,2)^2);
B = R*(Tc1(1,2)*Tc1(1,1)+Tc1(2,2)*Tc1(2,1));
C = -R*(Tc1(1,2)*Tc1(1,1)+Tc1(2,2)*Tc1(2,1));
D = e*Tc1(3,1);
E = -e*Tc1(3,2);
u = C-E;
v = 2*D-2*A;
w = 4*B-2*C;
g = 2*A+2*D;
h = C+E;
% [u,v,w,g,h]
if(abs(u)<=1e-8)
    u = 1e-8;
end
if(abs(v)<=1e-8)
    v = 1e-8;
end
if(abs(w)<=1e-8)
    w = 1e-8;
end
if(abs(g)<=1e-8)
    g = 1e-8;
end
if(abs(h)<=1e-8)
    h = 1e-8;
end
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
for j=1:i
        theta = 2*atan(root(j));
        t1 = (R*Tc1(1,1)*cos(theta)+R*Tc1(1,2)*sin(theta)-c)/a;
        t2 = (R*Tc1(2,1)*cos(theta)+R*Tc1(2,2)*sin(theta)-d)/b;
        dis2 = YS_getSharpPointToCircle(t1,t2,length,width,R,Tc1);
        if(dis2<dmin)
           dmin = dis2;
        end
        if(dmin<1e-5)
            return
        end
        if(t1>1)
            t1 = 1;
            [dmin1,P,Q] = YS_distanceCircleToLine(R,Tc1,[length,width,0],[length,0,0]);
            if(dmin1<dmin)
                P1 = P;
                Q1 = Q;
                dmin = dmin1;
            end
        end
          if(t1<0)
            t1 = 0;
            [dmin1,P,Q] = YS_distanceCircleToLine(R,Tc1,[0,0,0],[0,width,0]);
            if(dmin1<dmin)                
                P1 = P;
                Q1 = Q;
                dmin = dmin1;
            end
            end
         if(t2>1)
            t2=1;
            [dmin1,P,Q] = YS_distanceCircleToLine(R,Tc1,[0,width,0],[length,width,0]);
            if(dmin1<dmin)
                P1 = P;
                Q1 = Q;
                dmin = dmin1;
            end
            end
          if(t2<0)
            t2 = 0;
            [dmin1,P,Q] = YS_distanceCircleToLine(R,Tc1,[0,0,0],[length,0,0]);
            if(dmin1<dmin)
                P1 = P;
                Q1 = Q;
                dmin = dmin1;
             end
          end
        X = P3(1)+t1*a;
        Y = P3(2)+t2*b;
        Z = 0;
        Rx = R*Tc1(1,1)*cos(theta)+R*Tc1(1,2)*sin(theta)+Tc1(1,4);
        Ry = R*Tc1(2,1)*cos(theta)+R*Tc1(2,2)*sin(theta)+Tc1(2,4);
        Rz = R*Tc1(3,1)*cos(theta)+R*Tc1(3,2)*sin(theta)+Tc1(3,4);
        distance = sqrt((Rx-X)^2+(Ry-Y)^2+(Rz-Z)^2);
        if(dmin>distance)
            dmin = distance;
            Q = [Rx,Ry,Rz];
            P = [X,Y,Z];
        end
end   
end

测试代码

length = 100;
width = 100;
r = 50;
R = [1,0,0;
     0,1,0;
     0,0,1];
P = [21.451052,	20.256481,	24.921350]';
Tc1 =[R,P];
dis1=YS_distanceCircleToPlane(length,width,r,Tc1);