distanceLines3d

DISTANCELINES3D Minimal distance between two 3D lines.

   D = distanceLines3d(LINE1, LINE2);
   Returns the distance between line LINE1 and the line LINE2, given as:
   LINE1 : [x0 y0 z0 dx dy dz] (or M-by-6 array)
   LINE2 : [x0 y0 z0 dx dy dz] (or N-by-6 array)
   D     : (positive) array M-by-N

   [D, PT1, PT2] = distanceLines3d(LINE1, LINE2);
   Also returns the points located on LINE1 and LINE2 corresponding to the
   shortest distance. 
   One should get the following:
   distancePoints3d(PT1, PT2) - D == 0


   Example
     line1 = [2 3 4 0 1 0];
     line2 = [8 8 8 0 0 1];
     distanceLines3d(line1, line2)
     ans = 
         6.0000

   See also
   lines3d, distancePoints3d

0001 function [d, pt1, pt2] = distanceLines3d(line1, line2)
0002 %DISTANCELINES3D Minimal distance between two 3D lines.
0003 %
0004 %   D = distanceLines3d(LINE1, LINE2);
0005 %   Returns the distance between line LINE1 and the line LINE2, given as:
0006 %   LINE1 : [x0 y0 z0 dx dy dz] (or M-by-6 array)
0007 %   LINE2 : [x0 y0 z0 dx dy dz] (or N-by-6 array)
0008 %   D     : (positive) array M-by-N
0009 %
0010 %   [D, PT1, PT2] = distanceLines3d(LINE1, LINE2);
0011 %   Also returns the points located on LINE1 and LINE2 corresponding to the
0012 %   shortest distance.
0013 %   One should get the following:
0014 %   distancePoints3d(PT1, PT2) - D == 0
0015 %
0016 %
0017 %   Example
0018 %     line1 = [2 3 4 0 1 0];
0019 %     line2 = [8 8 8 0 0 1];
0020 %     distanceLines3d(line1, line2)
0021 %     ans =
0022 %         6.0000
0023 %
0024 %   See also
0025 %   lines3d, distancePoints3d
0026 
0027 % ------
0028 % Authors: Brandon Baker, oqilipo, David Legland
0029 % E-mail: david.legland@inrae.fr
0030 % Created: 2011-01-19
0031 % Copyright 2011-2024
0032 
0033 % number of points of each array
0034 n1 = size(line1, 1);
0035 n2 = size(line2, 1);
0036 
0037 if nargout <= 1
0038     % express line coordinate as n1-by-n2 arrays
0039     v1x = repmat(line1(:,4), [1 n2]);
0040     v1y = repmat(line1(:,5), [1 n2]);
0041     v1z = repmat(line1(:,6), [1 n2]);
0042     p1x = repmat(line1(:,1), [1 n2]);
0043     p1y = repmat(line1(:,2), [1 n2]);
0044     p1z = repmat(line1(:,3), [1 n2]);
0045 
0046     v2x = repmat(line2(:,4)', [n1 1]);
0047     v2y = repmat(line2(:,5)', [n1 1]);
0048     v2z = repmat(line2(:,6)', [n1 1]);
0049     p2x = repmat(line2(:,1)', [n1 1]);
0050     p2y = repmat(line2(:,2)', [n1 1]);
0051     p2z = repmat(line2(:,3)', [n1 1]);
0052 
0053     % calculates distance for each set of lines
0054     vcross = cross([v1x(:) v1y(:) v1z(:)], [v2x(:) v2y(:) v2z(:)]);
0055     num = ([p1x(:) p1y(:) p1z(:)] - [p2x(:) p2y(:) p2z(:)]) .* vcross;
0056     t1 = sum(num,2);
0057     d = abs(t1) ./ (vectorNorm3d(vcross) + eps);
0058     
0059     % returns result as n1-by-n2 array
0060     d = reshape(d, n1, n2);
0061 
0062 else
0063     % check input dimension, as we need to be able to match each pair of
0064     % lines
0065     if n1 ~= n2
0066         error('geom3d:distanceLines3d:IllegalInputArgument', ...
0067             'when output points are requested, number of lines should be the same');
0068     end
0069     
0070     p1 = line1(:, 1:3);
0071     p2 = line2(:, 1:3);
0072     dp = p2 - p1;
0073     v1 = line1(:, 4:6);
0074     v2 = line2(:, 4:6);
0075 
0076     % compute distance
0077     vcross = cross(v1, v2, 2);
0078     num = dp .* vcross;
0079     t1 = sum(num, 2);
0080     d = abs(t1) ./ (vectorNorm3d(vcross) + eps);
0081 
0082     % precomputations
0083     a = dot(v1, v1, 2);
0084     b = dot(v1, v2, 2);
0085     e = dot(v2, v2, 2);
0086     den = a.*e - b.*b; % 0, if lines are parallel
0087     
0088     % vector between origin of both lines
0089     r = line1(:,1:3) - line2(:,1:3);
0090     
0091     % solve linear system
0092     c = dot(v1, r, 2);
0093     f = dot(v2, r, 2);
0094     s = (b .* f - c .* e) ./ den;
0095     t = (a .* f - c .* b) ./ den;
0096 
0097     % convert to coordinates of points on lines
0098     pt1 = line1(:,1:3) + v1 .* s;
0099     pt2 = line2(:,1:3) + v2 .* t;
0100 end