function point = intersectEdges(edge1, edge2, varargin)
%INTERSECTEDGES Return all intersections between two set of edges.
%
%   P = intersectEdges(E1, E2);
%   returns the intersection point of edges E1 and E2. 
%   E1 and E2 are 1-by-4 arrays, containing parametric representation of
%   each edge (in the form [x1 y1 x2 y2], see 'createEdge' for details).
%   
%   In case of colinear edges, the result P contains [Inf Inf].
%   In case of parallel but not colinear edges, the result P contains 
%   [NaN NaN]. 
%
%   If each input is N-by-4 array, the result is a N-by-2 array containing
%   the intersection of each couple of edges.
%   If one of the input has N rows and the other 1 row, the result is a
%   N-by-2 array.
%
%   P = intersectEdges(E1, E2, TOL);
%   Specifies a tolerance parameter to determine parallel and colinear
%   edges, and if a point belongs to an edge or not. The latter test is
%   performed on the relative position of the intersection point over the
%   edge, that should lie within [-TOL; 1+TOL]. 
%
%   See also:
%   edges2d, intersectLines
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% INRA - Cepia Software Platform
% created the 31/10/2003.
%

%   HISTORY
%   19/02/2004 add support for multiple edges.
%   15/08/2005 rewrite a lot, and create unit tests
%   08/03/2007 update doc
%   21/09/2009 fix bug for edge arrays (thanks to Miquel Cubells)
%   03/08/2010 fix another bug for edge arrays (thanks to Reto Zingg)
%   20/02/2013 fix bug for management of parallel edges (thanks to Nicolaj
%   Kirchhof)
%   19/07/2016 add support for tolerance

% tolerance for precision
tol = 1e-14;

if nargin > 2
    tol = varargin{1};
end


%% Initialisations

% ensure input arrays are same size
N1  = size(edge1, 1);
N2  = size(edge2, 1);

% ensure input have same size
if N1 ~= N2
    if N1 == 1
        edge1 = repmat(edge1, [N2 1]);
        N1 = N2;
    elseif N2 == 1
        edge2 = repmat(edge2, [N1 1]);
    end
end

% initialize result array
x0  = zeros(N1, 1);
y0  = zeros(N1, 1);


%% Detect parallel and colinear cases

% indices of parallel edges
%par = abs(dx1.*dy2 - dx1.*dy2)<tol;
par = isParallel(edge1(:,3:4)-edge1(:,1:2), edge2(:,3:4)-edge2(:,1:2));

% indices of colinear edges
% equivalent to: |(x2-x1)*dy1 - (y2-y1)*dx1| < eps
col = abs(  (edge2(:,1)-edge1(:,1)) .* (edge1(:,4)-edge1(:,2)) - ...
            (edge2(:,2)-edge1(:,2)) .* (edge1(:,3)-edge1(:,1)) ) < tol & par;

% Parallel edges have no intersection -> return [NaN NaN]
x0(par & ~col) = NaN;
y0(par & ~col) = NaN;


%% Process colinear edges

% colinear edges may have 0, 1 or infinite intersection
% Discrimnation based on position of edge2 vertices on edge1
if sum(col) > 0
    % array for storing results of colinear edges
    resCol = Inf * ones(size(col));

    % compute position of edge2 vertices wrt edge1
    t1 = edgePosition(edge2(col, 1:2), edge1(col, :));
    t2 = edgePosition(edge2(col, 3:4), edge1(col, :));
    
    % control location of vertices: we want t1<t2
    if t1 > t2
        tmp = t1;
        t1  = t2;
        t2  = tmp;
    end
    
    % edge totally before first vertex or totally after last vertex
    resCol(col(t2 < -tol))  = NaN;
    resCol(col(t1 > 1+tol)) = NaN;
        
    % set up result into point coordinate
    x0(col) = resCol(col);
    y0(col) = resCol(col);
    
    % touches on first point of first edge
    touch = col(abs(t2) < tol);
    x0(touch) = edge1(touch, 1);
    y0(touch) = edge1(touch, 2);

    % touches on second point of first edge
    touch = col(abs(t1-1) < tol);
    x0(touch) = edge1(touch, 3);
    y0(touch) = edge1(touch, 4);
end


%% Process non parallel cases

% process edges whose supporting lines intersect
i = find(~par);

% use a test to avoid process empty arrays
if sum(i) > 0
    % extract base parameters of supporting lines for non-parallel edges
    x1  = edge1(i,1);
    y1  = edge1(i,2);
    dx1 = edge1(i,3) - x1;
    dy1 = edge1(i,4) - y1;
    x2  = edge2(i,1);
    y2  = edge2(i,2);
    dx2 = edge2(i,3) - x2;
    dy2 = edge2(i,4) - y2;

    % compute intersection points of supporting lines
    delta = (dx2.*dy1 - dx1.*dy2);
    x0(i) = ((y2-y1).*dx1.*dx2 + x1.*dy1.*dx2 - x2.*dy2.*dx1) ./ delta;
    y0(i) = ((x2-x1).*dy1.*dy2 + y1.*dx1.*dy2 - y2.*dx2.*dy1) ./ -delta;
        
    % compute position of intersection points on each edge
    % t1 is position on edge1, t2 is position on edge2
    t1  = ((y0(i)-y1).*dy1 + (x0(i)-x1).*dx1) ./ (dx1.*dx1+dy1.*dy1);
    t2  = ((y0(i)-y2).*dy2 + (x0(i)-x2).*dx2) ./ (dx2.*dx2+dy2.*dy2);

    % check position of points on edges.
    % it should be comprised between 0 and 1 for both t1 and t2.
    % if not, the edges do not intersect
    out = t1<-tol | t1>1+tol | t2<-tol | t2>1+tol;
    x0(i(out)) = NaN;
    y0(i(out)) = NaN;
end


%% format output arguments

point = [x0 y0];

end

function b = isParallel(v1, v2, varargin)
%ISPARALLEL Check parallelism of two vectors.
%
%   B = isParallel(V1, V2)
%   where V1 and V2 are two row vectors of length ND, ND being the
%   dimension, returns 1 if the vectors are parallel, and 0 otherwise.
%
%   Also works when V1 and V2 are two N-by-ND arrays with same number of
%   rows. In this case, return a N-by-1 array containing 1 at the positions
%   of parallel vectors.
%
%   Also works when one of V1 or V2 is N-by-1 and the other one is N-by-ND
%   array, in this case return N-by-1 results.
%
%   B = isParallel(V1, V2, ACCURACY)
%   specifies the accuracy for numerical computation. Default value is
%   1e-14. 
%   
%
%   Example
%   isParallel([1 2], [2 4])
%   ans =
%       1
%   isParallel([1 2], [1 3])
%   ans =
%       0
%
%   See also
%   vectors2d, isPerpendicular, lines2d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-04-25
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   2007-09-18 copy from isParallel3d, adapt to any dimension, and add psb
%       to specify precision
%   2007-01-16 fix bug
%   2009-09-21 fix bug for array of 3 vectors
%   2011-01-20 replace repmat by ones-indexing (faster)
%   2011-06-16 use direct computation (faster)
%   2017-08-31 use normalized vectors

% default accuracy
acc = 1e-14;
if ~isempty(varargin)
    acc = abs(varargin{1});
end

% normalize vectors
v1 = normalizeVector(v1);
v2 = normalizeVector(v2);

% adapt size of inputs if needed
n1 = size(v1, 1);
n2 = size(v2, 1);
if n1 ~= n2
    if n1 == 1
        v1 = v1(ones(n2,1), :);
    elseif n2 == 1
        v2 = v2(ones(n1,1), :);
    end
end

% performs computation
if size(v1, 2) == 2
    % computation for plane vectors
    b = abs(v1(:, 1) .* v2(:, 2) - v1(:, 2) .* v2(:, 1)) < acc;
else
    % computation in greater dimensions 
    b = vectorNorm(cross(v1, v2, 2)) < acc;
end

end

function vn = normalizeVector(v)
%NORMALIZEVECTOR Normalize a vector to have norm equal to 1.
%
%   V2 = normalizeVector(V);
%   Returns the normalization of vector V, such that ||V|| = 1. V can be
%   either a row or a column vector.
%
%   When V is a M-by-N array, normalization is performed for each row of
%   the array.
%
%   When V is a M-by-N-by-2 array, normalization is performed along the
%   last dimension of the array.
%
%   Example:
%   vn = normalizeVector([3 4])
%   vn =
%       0.6000   0.8000
%   vectorNorm(vn)
%   ans =
%       1
%
%   See Also:
%     vectors2d, vectorNorm
%

%   ---------
%   author : David Legland 
%   INRA - TPV URPOI - BIA IMASTE
%   created the 29/11/2004.
%

%   HISTORY
%   2005-01-14 correct bug
%   2009-05-22 rename as normalizeVector
%   2011-01-20 use bsxfun

if isvector(v)
    vn = v / norm(v);
elseif ismatrix(v)
    vn = v./ sqrt(sum(v.^2, 2));
else
    vn = v./ sqrt(sum(v.^2, ndims(v)));
end

end

function pos = edgePosition(point, edge)
%EDGEPOSITION Return position of a point on an edge.
%
%   POS = edgePosition(POINT, EDGE);
%   Computes position of point POINT on the edge EDGE, relative to the
%   position of edge vertices.
%   EDGE has the form [x1 y1 x2 y2],
%   POINT has the form [x y], and is assumed to belong to edge.
%   The result POS has the following meaning:
%     POS < 0:      POINT is located before the first vertex
%     POS = 0:      POINT is located on the first vertex
%     0 < POS < 1:  POINT is located between the 2 vertices (on the edge)
%     POS = 1:      POINT is located on the second vertex
%     POS < 0:      POINT is located after the second vertex
%
%   POS = edgePosition(POINT, EDGES);
%   If EDGES is an array of NL edges, return NE positions, corresponding to
%   each edge.
%
%   POS = edgePosition(POINTS, EDGE);
%   If POINTS is an array of NP points, return NP positions, corresponding
%   to each point.
%
%   POS = edgePosition(POINTS, EDGES);
%   If POINTS is an array of NP points and EDGES is an array of NE edges,
%   return an array of [NP NE] position, corresponding to each couple
%   point-edge.
%
%   See also:
%   edges2d, createEdge, isPointOnEdge
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2004-05-25
% Copyright 2009 INRA - Cepia Software Platform.
%

%   HISTORY:
%   06/12/2009 created from linePosition

% number of points and of edges
nEdges = size(edge, 1);
nPoints = size(point, 1);

if nPoints == nEdges
    dxe = (edge(:, 3) - edge(:,1))';
    dye = (edge(:, 4) - edge(:,2))';
    dxp = point(:, 1) - edge(:, 1);
    dyp = point(:, 2) - edge(:, 2);

else
    % expand one of the arrays to have the same size
    dxe = (edge(:,3) - edge(:,1))';
    dye = (edge(:,4) - edge(:,2))';
    dxp = bsxfun(@minus, point(:,1), edge(:,1)');
    dyp = bsxfun(@minus, point(:,2), edge(:,2)');

end

pos = (dxp .* dxe + dyp .* dye) ./ (dxe .* dxe + dye .* dye);
end