%% Returns the intersection point of two lines stored as two-column matrices
%% ======================================================================================
%%
%% Parameters:
%%	l1, l2 - lines represented by sequences of vertices on a 2-D plane (x,y). 
%%		Each row in these matrices is a vertex, and columns contain the x and y coordinates
%%
%% Returns:
%%	p - matrix; each row contains (x,y) coordinates of an intersection between l1 and l2
%%		If no intersection is found, returns empty p.
%%	lambda - auxiliary array containing fractional coordinates of the intersection point within
%%		the intersecting line segments

function [ p lambda ] = intersection(l1,l2)

  p = [];
  lambda = [];

  if size(l1,1) > 2	% first line contains more than one segment - do them separately

	for i=2:size(l1,1)
		p = [ p; intersection(l1((i-1):i,:),l2) ];
	end

	return
  end

  if size(l2,1) > 2	% second line contains more than one segment - do them separately

	for i=2:size(l2,1)
		p = [ p; intersection(l1,l2((i-1):i,:)) ];
	end

	return
  end

  if size(l1,1) < 2 || size(l2,1) < 2		% no segments, hence no intersection
	return
  end
	
	% if got here, have exactly one segment in each line

  dl1 = l1(2,:)-l1(1,:);	% increments within segments (rows)
  dl2 = l2(2,:)-l2(1,:);

	% form a 2 by 2 system of linear equations A*lambda = b to find the intersection
  
  A = 	[ dl1(1), -dl2(1)	% first row is the equation for equal X coordinates
	  dl1(2), -dl2(2)	% second is for Y coordinates
	];

  b = 	[ l2(1,1) - l1(1,1)
	  l2(1,2) - l1(1,2)
	];

  if rank(A) < 2	% lines are parallel, no intersection
	return
  end

  lambda = inv(A)*b;	% relative positions of intersection

	% the intersection is correct (within the segments) if both lambdas are between 0 and 1

  if lambda(1,1) >= 0.0 && lambda(1,1) <= 1.0 && lambda(2,1) >= 0.0 && lambda(2,1) <= 1.0 

	p = l1(1,:) + dl1*lambda(1,1);		% intersection point

#l1
#l2
#lambda
#p

  end

end