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

Last updated on：3 years 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

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

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