用MATLAB求两条3D直线的交点

Last updated on:4 months ago

这个算法不是很难,我分了已知条件的不同分了两种求法。

已知直线上的两个点

下面的代码是转载的,里面有作者的信息。

function [x,y] = findintersection(L1,L2)

% findintersection is a function for finding the intersection point of two lines (vectors)
%   Inputs into this function are coordinates of two lines (vectors)
%   Line1 = [(x11,y11);(x12,y12)] and line2=[(x21,y21);(x22,y22)]
%   Example1 [xintersect yintersect] = findintersection([0.5 1;1 0],[0.5 0;1 1])
%   Example2 [xintersect yintersect]= findintersection([15 0;100 200],[50 0;50 200])
%   Example3 [xintersect yintersect]= findintersection([0 50;100 50],[50 0;50 100])
%   Outputs are x and y positions of the intersection point
%   Written by L M Vhengani
%   2011 September
%   Email: Lvhengani@gmail.com
%   Reference: http://mathworld.wolfram.com/Line-LineIntersection.html

x1=L1(1,1);y1=L1(1,2);x2=L1(2,1);y2=L1(2,2); % Components of Line 1
x3=L2(1,1);y3=L2(1,2);x4=L2(2,1);y4=L2(2,2); % Components of Line 2



x = det([det([x1 y1;x2 y2]), (x1-x2);det([x3 y3;x4 y4]), (x3-x4) ])/det([(x1-x2),(y1-y2) ;(x3-x4),(y3-y4)]);

y = det([det([x1 y1;x2 y2]), (y1-y2);det([x3 y3;x4 y4]), (y3-y4) ])/det([(x1-x2),(y1-y2) ;(x3-x4),(y3-y4)]);


% figure 
% plot(L1(:,1),L1(:,2))
% hold all
% plot(L2(:,1),L2(:,2))
% 
% plot(x,y,'*r','markersize',5)
% legend({'line1','line2','intersection'},'Location','West')
% hold off
% grid on;

end

已知直线上的一点和一个法向量

这个稍微有点复杂,是我自己编写的。输入的四个参数都是1x3的矩阵,返回一个1x3的矩阵(交点)。

function [P_intersect] = lineIntersect3D(P1,P2,u,v)
% Find intersection point of lines in 3D space
% the intersection should exsist
a = abs(u(1))<=1e-7;
b = abs(u(2))<=1e-7;
c = abs(u(3))<=1e-7;
d = abs(v(1))<=1e-7;
e = abs(v(2))<=1e-7;
f = abs(v(3))<=1e-7;
if (a&&b&&c)||(abs(sum(P1-P2))<=1e-7)||(d&&e&&f)
    P_intersect = P1;
    return
end
tmp1 = 1;
tmp2 = 1;
tmp3 = 1;
if a
    x = P1(1);
elseif d
    x = P2(1);
else
    tmp1 = 0;
end
if b
    y = P1(2);
elseif e
    y = P2(2);
else
    tmp2 = 0;
end
if c
    z = P1(3);
elseif f
    z = P2(3);
else
    tmp3 = 0;
end
if tmp1 == 1
    if tmp2 == 1
        if ~a
            z = (x-P1(1))/u(1)*u(3) + P1(3);
        elseif ~b
            z = (y-P1(2))/u(2)*u(3) + P1(3);
        elseif ~d
            z = (x-P2(1))/v(1)*v(3) + P2(3);
        elseif ~e
            z = (y-P2(2))/v(2)*v(3) + P2(3);
        else
            disp("error")
            return %error
        end
    elseif tmp3 == 1
        if ~a
            y = (x-P1(1))/u(1)*u(2) + P1(2);
        elseif ~c
            y = (z-P1(3))/u(3)*u(2) + P1(2);
        elseif ~d
            y = (x-P2(1))/v(1)*v(2) + P2(2);
        elseif ~f
            y = (z-P2(3))/v(3)*v(2) + P2(2);
        else
            disp("error")
            return %error
        end
    else
        z = ((P2(2)-P1(2))*u(3)*v(3)+u(2)*v(3)*P1(3)-u(3)*v(2)*P2(3))/...
            (u(2)*v(3)-u(3)*v(2));
        y = (z-P1(3))/u(3)*u(2) + P1(2);
    end
elseif tmp2 == 1
    if tmp3 == 1
        if ~b
            x = (y-P1(2))/u(2)*u(1) + P1(1);
        elseif ~c
            x = (z-P1(3))/u(3)*u(1) + P1(1);
        elseif ~e
            x = (y-P2(2))/v(2)*v(1) + P2(1);
        elseif ~f
            x = (z-P2(3))/v(3)*v(1) + P2(1);
        else
            disp("error")
            return %error
        end
    else
        z = ((P2(1)-P1(1))*u(3)*v(3)+u(1)*v(3)*P1(3)-u(3)*v(1)*P2(3))/...
            (u(1)*v(3)-u(3)*v(1));
        x = (z-P1(3))/u(3)*u(1) + P1(1);
    end
elseif tmp3 == 1
    y = ((P2(1)-P1(1))*u(2)*v(2)+u(1)*v(2)*P1(2)-u(2)*v(1)*P2(2))/...
        (u(1)*v(2)-u(2)*v(1));
    x = (y-P1(2))/u(2)*u(1) + P1(1);
else
    z = ((P2(1)-P1(1))*u(3)*v(3)+u(1)*v(3)*P1(3)-u(3)*v(1)*P2(3))/...
        (u(1)*v(3)-u(3)*v(1));
    x = (z-P1(3))/u(3)*u(1) + P1(1);
    y = (z-P1(3))/u(3)*u(2) + P1(2);
end

P_intersect = [x y z];
% figure
% L1 = [P1;P_intersect];
% L2 = [P2;P_intersect];
% Lu1 = [0 0 0;u];
% Lu2 = [0 0 0;v];
% hold on
% plot3(L1(:,1),L1(:,2),L1(:,3));
% hold on
% plot3(L2(:,1),L2(:,2),L2(:,3));
% hold on
% plot3(Lu1(:,1),Lu1(:,2),Lu1(:,3));
% hold on
% plot3(Lu2(:,1),Lu2(:,2),Lu2(:,3));
end