function [intersection,status] = cv_int_line_line(line1,line2,dist_eps)
%
% cv_int_line_line - whether and where two lines intersect
% Call: [intersection,status] = cv_int_line_line(line1,line2);
% On input:
%     line1: 2 points defining line (matrix 2x3)
%     line2: 2 points defining line (matrix 2x3)
%     dist_eps: distance considered close enough for intersection
% On output:
%     intersection: 2 points  if intersect in line (matrix 2x3)
%                   inf       if lines don't intersect
%                   3D point  if lines intersect
%     status: -1   if lines are same line
%              0   if lines do not intersect
%              1   if lines intersect in a single point
% Author:
%     Tom Henderson
%     7 January 2000
% Custom Functions Used:
%     none
% Method:
%     find parameters of points on 2 lines that minimize distance 
% Testing:
% > cv_int_line_line([0 0 0; 1 0 0], [0 0 0; 0 1 0],0.01)
%
% ans =
%
%     0     0     0
%
% > cv_int_line_line([0 0 0; 1 0 0], [0 0 1; 0 1 1],0.01)
%
% ans =
%
%   Inf
%
% > cv_int_line_line([0 0 0; 1 1 0], [1 0 0; 0 1 0],0.01)
%
% ans =
%
%    0.5000    0.5000         0
%
% > cv_int_line_line([0 0 0; 1 0 0], [0 0 0; 1 0 0],0.01)
%
% ans =
%
%     0     0     0
%     1     0     0
%

if ((cv_dist_pt_line(line1(1,:),line2) < dist_eps) ...   % same line
      & (cv_dist_pt_line(line1(2,:),line2) < dist_eps))
   intersection = line1;
   status = -1;
   return
end

d = cv_dist_line_line(line1,line2);
if (d<dist_eps)      % lines intersect in a point
   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
   intersection = p;
   status = 1;
else    % lines do not intersect
   intersection = inf;
   status = 0;
end
