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

   ---------
   authors: Brandon Baker, oqilipo, David Legland
   created January 19, 2011

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 %   created January 19, 2011
0030 %
0031 
0032 % number of points of each array
0033 n1 = size(line1, 1);
0034 n2 = size(line2, 1);
0035 
0036 if nargout <= 1
0037     % express line coordinate as n1-by-n2 arrays
0038     v1x = repmat(line1(:,4), [1 n2]);
0039     v1y = repmat(line1(:,5), [1 n2]);
0040     v1z = repmat(line1(:,6), [1 n2]);
0041     p1x = repmat(line1(:,1), [1 n2]);
0042     p1y = repmat(line1(:,2), [1 n2]);
0043     p1z = repmat(line1(:,3), [1 n2]);
0044 
0045     v2x = repmat(line2(:,4)', [n1 1]);
0046     v2y = repmat(line2(:,5)', [n1 1]);
0047     v2z = repmat(line2(:,6)', [n1 1]);
0048     p2x = repmat(line2(:,1)', [n1 1]);
0049     p2y = repmat(line2(:,2)', [n1 1]);
0050     p2z = repmat(line2(:,3)', [n1 1]);
0051 
0052     % calculates distance for each set of lines
0053     vcross = cross([v1x(:) v1y(:) v1z(:)], [v2x(:) v2y(:) v2z(:)]);
0054     num = ([p1x(:) p1y(:) p1z(:)] - [p2x(:) p2y(:) p2z(:)]) .* vcross;
0055     t1 = sum(num,2);
0056     d = abs(t1) ./ (vectorNorm3d(vcross) + eps);
0057     
0058     % returns result as n1-by-n2 array
0059     d = reshape(d, n1, n2);
0060 
0061 else
0062     % check input dimension, as we need to be able to match each pair of
0063     % lines
0064     if n1 ~= n2
0065         error('geom3d:distanceLines3d:IllegalInputArgument', ...
0066             'when output points are requested, number of lines should be the same');
0067     end
0068     
0069     p1 = line1(:, 1:3);
0070     p2 = line2(:, 1:3);
0071     dp = p2 - p1;
0072     v1 = line1(:, 4:6);
0073     v2 = line2(:, 4:6);
0074 
0075     % compute distance
0076     vcross = cross(v1, v2, 2);
0077     num = dp .* vcross;
0078     t1 = sum(num, 2);
0079     d = abs(t1) ./ (vectorNorm3d(vcross) + eps);
0080 
0081     % precomputations
0082     a = dot(v1, v1, 2);
0083     b = dot(v1, v2, 2);
0084     e = dot(v2, v2, 2);
0085     den = a.*e - b.*b; % 0, if lines are parallel
0086     
0087     % vector between origin of both lines
0088     r = line1(:,1:3) - line2(:,1:3);
0089     
0090     % solve linear system
0091     c = dot(v1, r, 2);
0092     f = dot(v2, r, 2);
0093     s = (b .* f - c .* e) ./ den;
0094     t = (a .* f - c .* b) ./ den;
0095 
0096     % convert to coordinates of points on lines
0097     pt1 = line1(:,1:3) + v1 .* s;
0098     pt2 = line2(:,1:3) + v2 .* t;
0099 end