% Computes the minimum distance between two line segments. Code
% is adapted for Matlab from Dan Sunday's Geometry Algorithms originally
% written in C++
% http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm#dist3D_Segment_to_Segment
% Usage: Input the start and end x,y,z coordinates for two line segments.
% p1, p2 are [x,y,z] coordinates of first line segment and p3,p4 are for
% second line segment.
% Output: scalar minimum distance between the two segments.
% Example:
% P1 = [0 0 0]; P2 = [1 0 0];
% P3 = [0 1 0]; P4 = [1 1 0];
% dist = DistBetween2Segment(P1, P2, P3, P4)
% dist =
%
% 1
%
function [distance varargout] = DistBetween2Segment(p1, p2, p3, p4)
%p1 = [13 31 25];
%p2 = [36 34 40];
%p3 = [55 35 32];
%p4 = [4 47 62]; %test
distance = zeros(size(p1,1),1);%size(p1,1) returns the number of rows of p1, so size(p1,1)=1;
sN = zeros(size(p1,1),1);%zeros(m,n) returns 0 with m rows and n arrays;
tN = zeros(size(p1,1),1);
sc = zeros(size(p1,1),1);
tc = zeros(size(p1,1),1);
dP = zeros(size(p1,1),3);
u = p1 - p2;
v = p3 - p4;
w = p2 - p4;
a = dot(u,u,2);%向量的点积u.u
b = dot(u,v,2);
c = dot(v,v,2);
d = dot(u,w,2);
e = dot(v,w,2);
D = a.*c - b.*b; % ac -b^2
sD = D;
tD = D;
SMALL_NUM = 0.00000001;
% compute the line parameters of the two closest points
for n=1:size(p1,1);
if (D(n,1) < SMALL_NUM) % the lines are almost parallel
sN(n,1) = 0.0; % force using point P0 on segment S1
sD(n,1) = 1.0; % to prevent possible division by 0.0 later
tN(n,1) = e(n,1);
tD(n,1) = c(n,1);
else % get the closest points on the infinite lines
sN(n,1) = (b(n,1)*e(n,1) - c(n,1)*d(n,1));
tN(n,1) = (a(n,1)*e(n,1) - b(n,1)*d(n,1));
if (sN(n,1) < 0.0) % sc < 0 => the s=0 edge is visible
sN(n,1) = 0.0;
tN(n,1) = e(n,1);
tD(n,1) = c(n,1);
elseif (sN(n,1) > sD(n,1))% sc > 1 => the s=1 edge is visible
sN(n,1) = sD(n,1);
tN(n,1) = e(n,1) + b(n,1);
tD(n,1) = c(n,1);
end
end
if (tN(n,1) < 0.0) % tc < 0 => the t=0 edge is visible
tN(n,1) = 0.0;
% recompute sc for this edge
if (-d(n,1) < 0.0)
sN(n,1) = 0.0;
elseif (-d(n,1) > a(n,1))
sN(n,1) = sD(n,1);
else
sN(n,1) = -d(n,1);
sD(n,1) = a(n,1);
end
elseif (tN(n,1) > tD(n,1)) % tc > 1 => the t=1 edge is visible
tN(n,1) = tD(n,1);
% recompute sc for this edge
if ((-d(n,1) + b(n,1)) < 0.0)
sN(n,1) = 0;
elseif ((-d(n,1) + b(n,1)) > a(n,1))
sN(n,1) = sD(n,1);
else
sN(n,1) = (-d(n,1) + b(n,1));
sD(n,1) = a(n,1);
end
end
% finally do the division to get sc and tc
if(abs(sN(n,1)) < SMALL_NUM)
sc(n,1) = 0.0;
else
sc(n,1) = sN(n,1) / sD(n,1);
end
if(abs(tN(n,1)) < SMALL_NUM)
tc(n,1) = 0.0;
else
tc(n,1) = tN(n,1) / tD(n,1);
end
% get the difference of the two closest points
dP(n,:) = w(n,:) + (sc(n,1) * u(n,:)) - (tc(n,1) * v(n,:)); % = S1(sc) - S2(tc)
distance(n,1) = norm(dP(n,:));
end
% for m = 1:size(p1,1)
% distance(m,1) = norm(dP(m,:));
% end
outV = dP;
varargout(1) = {outV}; % vector connecting the closest points
%varargout(2) = {p2+sc*u}; % Closest point on object 1
%varargout(3) = {p4+tc*v}; % Closest point on object 2
end