You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
357 lines
9.5 KiB
357 lines
9.5 KiB
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
|