理论推导

matlab代码

function [dmin] = distanceConeToLine (A1,B1,A2,B2,R)
dmin = 100000000;
h = norm(A2-B2);
A = B1(1)-A1(1);
if(abs(A)<1e-2)
    A = 1e-2;
end
B = B1(2)-A1(2);
if(abs(B)<1e-2)
    B = 1e-2;
end
C = B1(3)-A1(3);
F = A1(1)*C+A*h-A1(3)*A;
G = A1(2)*C+B*h-A1(3)*B;
H = G*A*h-B*h*F;
J = G*R*C;
K = C*R*F;

P = H-J;
Q = -2*K;
N = J + H;
[root,y,i]= Solve2OrderEquaton([P,Q,N]);
for j=1:i
    theta = 2*atan(root(j));
    cT = cos(theta);
    sT = sin(theta);
    t = F/(R*C*cT+A*h);
    s = (h-h*t-A1(3))/C;
    if(t>1)
        t=1;
    end
    if(t<0)
        t = 0;
    end
    if(s>1)
        s=1;
    end
    if(s<0)
        s = 0;
    end
    Rx = R*t*cT;
    Ry = R*t*sT;
    Rz = h-h*t;
    Px = A1(1)+A*s;
    Py = A1(2)+B*s;
    Pz = A1(3)+C*s;
    d = sqrt((Rx-Px)^2+(Ry-Py)^2+(Rz-Pz)^2);
    if(dmin>d)
         dmin = d;
    end
end
if(dmin<1e-5)
    dmin = 0;
    return;
end
D = h + R*R;
E = C/h;
u = E*R-A;
v = 2*B;
w = 2*E*R;
m = 2*B;
n = A+E*R;
if(u == 0&&v==0&&w==0)
    root = 0;
    i = 1;
else if(u == 0&&v==0)
  [root,y,i]= Solve2OrderEquaton([w,m,n]);
else if(u == 0)
  [root,y,i]= Solve3OrderEquaton([v,w,m,n]);
else
  [root,y,i]= Solve4OrderEquaton([u,v,w,m,n]);
end
end
end
for j=1:i
    theta = 2*atan(root(j));
    cT = cos(theta);
    sT = sin(theta);
    tT = tan(theta);
    s = (A1(2)-A1(1)*tT)/(A*tT-B);
    t = (R*(A1(1)*cT+A*s*cT+A1(2)*sT+B*s*sT)-h*(A1(3)+C*s-h))/(h*h+R*R);
    if(t>1)
        t=1;
    end
    if(t<0)
        t = 0;
    end
    if(s>1)
        s=1;
    end
    if(s<0)
        s = 0;
    end
    Rx = R*t*cT;
    Ry = R*t*sT;
    Rz = h-h*t;
    Px = A1(1)+A*s;
    Py = A1(2)+B*s;
    Pz = A1(3)+C*s;
    d = sqrt((Rx-Px)^2+(Ry-Py)^2+(Rz-Pz)^2);
    if(dmin>d)
         dmin = d;
    end
end

测试代码

A1 = [1,1,1];
B1 = [10,10,2];		
C1 = [0,0,0];
D1 = [0,0,20];
R = 40;
dis1 = distanceConeToLine(A1,B1,C1,D1,R)

测试结果