function d = cv_dist_line_plane(line, plane)
%
% cv_dist_line_plane - distance from line to plane
% Call: d = cv_dist_line_plane(line,plane);
% On input:
%     line: 2 points (matrix of 2x3)
%     plane: 3 points (matrix of 3x3)
% On output:
%     d: minimum distance from line to plane
% Author:
%     Tom Henderson
%     7 January 2000
% Custom Functions Used:
%     none
% Method:
%     find parameter of line and parameters of plane that minimize distance
% between point on plane and point on line
% Testing:
% > d(1,1) = cv_dist_line_plane([0 0 0; 1 0 0], [0 0 0; 1 0 0; 0 1 0]);
% Warning: Matrix is singular to working precision.
% > In C:\MATLABR11\work\cv_dist_line_plane.m at line 31
% > d(1,2) = cv_dist_line_plane([0 0 0; 1 0 0], [1 0 0; 1 1 0; 1 0 1]);
% > d(1,3) = cv_dist_line_plane([0 0 0; 0 1 0], [1 0 0; 1 1 0; 1 0 1]);
% Warning: Matrix is singular to working precision.
% > In C:\MATLABR11\work\cv_dist_line_plane.m at line 31
% > d(1,4) = cv_dist_line_plane([0 0 0; 0 1 0], [1 0 0; 1 1 0; 0 0 1]);
% Warning: Matrix is singular to working precision.
% > In C:\MATLABR11\work\cv_dist_line_plane.m at line 31
% > d
%
% d =
%
%         0         0    1.0000    0.7071
%
p0 = plane(1,:);
p1 = plane(2,:);
p2 = plane(3,:);
r0 = line(1,:);
r1 = line(2,:);
v = (p1-p0)';
w = (p2-p0)';
s = (r1-r0)';
A = [v w -s];
b = (p0-r0)';
u = A\b;
if (u(1)==inf) | (u(2)==inf) | (u(3)==inf)  % if line parallel to plane
   d = cv_dist_pt_plane(line(1,:),plane);
   return
else    % else they intersect
   d = 0;
   return
end

%plane_pt = p0 + u(1)*v + u(2)*w;
%line_pt = r0 + u(3)*s;
%mid_pt = (plane_pt+line_pt)/2;
%d = 2*cv_dist_pt_line(mid_pt,line);
