function [d,p1,p] = LEM_dist_line_line(line1, line2)
%
% LEM_dist_line_line - distance between 2 lines
% Call: d = cv_dist_line_line(line1,line2);
% On input:
%     line1: line defined by 2 points (matrix is 2x3)
%     line2: line defined by 2 points (matrix is 2x3)
% On output:
%     d (float): minimum distance between the 2 lines
%     p1 (1x3 vector): point on line 1 closest to line 2
%     p2 (1x3 vector): point on line 2 closest to line 1
% Author:
%     Tom Henderson
%     7 January 2000
%     29 Dec 2020   revised TCH
% Custom Functions Used:
%     cv_dist_pt_line
% Method:
%     find point on each line that minimizes distance
% Testing:
% > d(1,1) = LEM_dist_line_line([0 0 0; 1 0 0],[0 0 0; 1 0 0]);
% Warning: Rank deficient, rank = 1  tol =   6.6613e-016.
% > In C:\MATLABR11\work\cv_dist_line_line.m at line 31
% > d(1,2) = LEM_dist_line_line([0 0 0; 1 0 0],[0 0 0; 0 1 0]);
% > d(1,3) = LEM_dist_line_line([0 0 0; 0 1 0],[1 0 0; 0 0 1]);
% > d(1,4) = LEM_dist_line_line([0 0 0; 1 0 0],[0 0 2; 1 0 2]);
% Warning: Rank deficient, rank = 1  tol =   6.6613e-016.
% > In C:\MATLABR11\work\cv_dist_line_line.m at line 31
% > d
%
% d =
%
%         0         0    0.7071    2.0000
%
b = zeros(2,1);
A = zeros(2,2);
b(1) = line2(1,1) - line1(1,1);
b(2) = line2(1,2) - line1(1,2);
b(3) = line2(1,3) - line1(1,3);
A(1,1) = line1(2,1)-line1(1,1);
A(1,2) = -(line2(2,1)-line2(1,1));
A(2,1) = line1(2,2)-line1(1,2);
A(2,2) = -(line2(2,2)-line2(1,2));
A(3,1) = line1(2,3)-line1(1,3);
A(3,2) = -(line2(2,3)-line2(1,3));
g = A\b;   % find parameters for both lines
p1 = line1(1,:) + g(1)*(line1(2,:)-line1(1,:));   % find point on line 1
p2 = line2(1,:) + g(2)*(line2(2,:)-line2(1,:));   % find point on line 2
p = (p1+p2)/2;    % get midpoint
d = 2*LEM_dist_pt_line(p,line1);   % distance is twice distance from midpoint to line
