www.gusucode.com > robot-9 源码程序matlab代码 > robot-9.4Toolbox/rvctools/common/Polygon.m
%POLYGON - General polygon class
%
% p = Polygon(vertices);
%
%
% Methods::
% plot Plot polygon
% area Area of polygon
% moments Moments of polygon
% centroid Centroid of polygon
% perimeter Perimter of polygon
% transform Transform polygon
% inside Test if points are inside polygon
% intersection Intersection of two polygons
% difference Difference of two polygons
% union Union of two polygons
% xor Exclusive or of two polygons
% display print the polygon in human readable form
% char convert the polgyon to human readable string
%
% Properties::
% vertices List of polygon vertices, one per column
% extent Bounding box [minx maxx; miny maxy]
% n Number of vertices
%
% Notes::
% - this is reference class object
% - Polygon objects can be used in vectors and arrays
%
% Acknowledgement::
%
% The methods inside, intersection, difference, union, and xor are based on code
% written by:
% Kirill K. Pankratov, kirill@plume.mit.edu,
% http://puddle.mit.edu/~glenn/kirill/saga.html
% and require a licence. However the author does not respond to email regarding
% the licence, so use with care.
% TODO
% split the code in two. Simple polygon functions in Polgon class, subclass with
% Pankratov code to Polygon2.
classdef Polygon < handle
properties
vertices
extent
end
properties (Dependent=true)
n
x
y
end
methods
function p = Polygon(v, wh)
%Polygon.Polygon Polygon class constructor
%
% P = Polygon(V) is a polygon with vertices given by V, one column per
% vertex.
%
% P = Polygon(C, WH) is a rectangle centred at C with dimensions
% WH=[WIDTH, HEIGHT].
if nargin == 0
p.n = 0;
p.vertices = [];
return;
end
if nargin < 2
if numrows(v) ~= 2
error('vertices must have two rows');
end
p.vertices = v;
end
if nargin == 2
if length(v) ~= 2
error('first argument must be polygon centre');
end
if length(wh) ~= 2
error('second arugment must be width height');
end
p.vertices = [
v(1)-wh(1)/2 v(1)+wh(1)/2 v(1)+wh(1)/2 v(1)-wh(1)/2
v(2)-wh(2)/2 v(2)-wh(2)/2 v(2)+wh(2)/2 v(2)+wh(2)/2 ];
end
% compute the extent
p.extent(1,1) = min(p.x);
p.extent(1,2) = max(p.x);
p.extent(2,1) = min(p.y);
p.extent(2,2) = max(p.y);
end
function r = get.n(p)
r = numcols(p.vertices);
end
function r = get.x(p)
r = p.vertices(1,:)';
end
function r = get.y(p)
r = p.vertices(2,:)';
end
function r = set.n(p)
error('cant set property');
end
function r = set.x(p)
error('cant set property');
end
function r = set.y(p)
error('cant set property');
end
function s = char(p)
%Polygon.char String representation
%
% S = P.char() is a compact representation of the polgyon in human
% readable form.
s = sprintf('%d vertices', p.n);
end
function display(p)
%Polygon.display Display polygon
%
% P.display() displays the polygon in a compact human readable form.
%
% See also Polygon.char.
loose = strcmp( get(0, 'FormatSpacing'), 'loose');
if loose
disp(' ');
end
disp([inputname(1), ' = '])
if loose
disp(' ');
end
disp(char(p))
if loose
disp(' ');
end
end
function plot(plist, varargin)
%Polygon.plot Plot polygon
%
% P.plot() plot the polygon.
%
% P.plot(LS) as above but pass the arguments LS to plot.
for p=plist
x = p.vertices(1,:)'; x = [x; x(1)];
y = p.vertices(2,:)'; y = [y; y(1)];
plot(x, y, varargin{:});
end
end
function a = area(p)
%Polygon.area Area of polygon
%
% A = P.area() is the area of the polygon.
a = p.moments(0, 0);
end
function m = moments(p, mp, mq)
%Polygon.moments Moments of polygon
%
% A = P.moments(p, q) is the pq'th moment of the polygon.
%
% See also mpq_poly.
m = mpq_poly(p.vertices, mp, mq);
end
function q = transform(p, T)
%Polygon.transform Transformation of polygon vertices
%
% P2 = P.transform(T) is a new Polygon object whose vertices have
% been transfored by the 3x3 homgoeneous transformation T.
if length(T) == 3
T = se2(T);
end
q = Polygon( homtrans(T, p.vertices) );
end
function f = inside(p, points)
%Polygon.inside Test if points are inside polygon
%
% IN = P.inside(P) tests if points given by columns of P are
% inside the polygon. The corresponding elements of IN are
% either true or false.
IN = inpolygon(points(1,:), points(2,:), p.x, p.y)
end
function c = centroid(p)
%Polygon.centroid Centroid of polygon
%
% X = P.centroid() is the centroid of the polygon.
xc = p.moments(1,1) / p.area();
end
function r = perimeter(p)
%Polygon.perimeter Perimeter of polygon
%
% L = P.perimeter() is the perimeter of the polygon.
p = sum(sqrt(diff(p.x).^2+diff(p.y).^2));
end
function f = intersect(p, plist)
%Polygon.intersect Intersection of polygon with list of polygons
%
% I = P.intersect(PLIST) indicates whether or not the Polygon P
% intersects with
%
% i(j) = 1 if p intersects polylist(j), else 0.
% Based on ISINTPL
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/20/95, 08/25/95
f = [];
for q=plist
f = [f isintpl(p.x, p.y, q.x, q.y)];
end
end
function f = intersect_line(p, l)
%Polygon.intersect_line Intersection of polygon and line segment
%
% I = P.intersect_line(L) is the intersection points of a polygon P
% with the line segment L=[x1 x2; y1 y2]. I is an Nx2 matrix with
% one column per intersection, each column is [x y]'.
f = [];
% find intersections
for i=1:p.n
in = mod(i, p.n)+1;
xv = [p.x(i); p.x(in)];
yv = [p.y(i); p.y(in)];
intsec = iscross(xv, yv, l(1,:)', l(2,:)');
if intsec
[x,y] = intsecl(xv, yv, l(1,:)', l(2,:)');
f = [f [x;y]];
end
end
end
function r = difference(p, q)
%Polygon.difference Difference of polygons
%
% D = P.difference(Q) is polygon P minus polygon Q.
%
% Notes::
% - If polygons P and Q are not intersecting, returns
% coordinates of P.
% - If the result D is not simply connected or consists of
% several polygons, resulting vertex list will contain NaNs.
% POLYDIFF Difference of 2 polygons.
% [XO,YO] = POLYDIFF(X1,Y1,X2,Y2) Calculates polygon(s) P
% of difference of polygons P1 and P1 with coordinates
% X1, Y1 and X2, Y2.
% The resulting polygon(s) is a set of all points which belong
% to P1 but not to to P2: P = P1 & ~P2.
% The input polygons must be non-self-intersecting and
% simply connected.
%
% If polygons P1, P2 are not intersecting, returns
% coordinates of the first polygon X1, X2.
% If the result P is not simply connected or consists of several
% polygons, resulting boundary consists of concatenated
% coordinates of these polygons, separated by NaN.
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/25/95
% Call POLYBOOL with flag=3
[xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 3);
r = Polygon([xo(:) yo(:)]');
end
function r = intersection(p, q)
%Polygon.intersection Intersection of polygons
%
% I = P.intersection(Q) is a Polygon representing the
% intersection of polygons P and Q.
%
% Notes::
% - If these polygons are not intersecting, returns empty polygon.
% - If intersection consist of several disjoint polygons
% (for non-convex P or Q) then vertices of I is the concatenation
% of the vertices of these polygons.
% POLYINTS Intersection of 2 polygons.
% [XO,YO] = POLYINTS(X1,Y1,X2,Y2) Calculates polygon(s)
% if intersection of polygons with coordinates X1, Y1
% and X2, Y2.
% The resulting polygon(s) is a set of all points which
% belong to both P1 and P2: P = P1 & P2.
% These polygons must be non-self-intersecting and
% simply connected.
%
% If these polygons are not intersecting, returns empty.
% If intersection consist of several disjoint polygons
% (for non-convex P1 or P2) output vectors XO, YO consist
% of concatenated cooddinates of these polygons,
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/25/95
% Call POLYBOOL with flag=1
[xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 1);
r = Polygon([xo(:) yo(:)]');
end
function r = union(p, q)
%Polygon.union Union of polygons
%
% I = P.union(Q) is a Polygon representing the
% union of polygons P and Q.
%
% Notes::
% - If these polygons are not intersecting, returns a polygon with
% vertices of both polygons separated by NaNs.
% - If the result P is not simply connected (such as a polygon
% with a "hole") the resulting contour consist of counter-
% clockwise "outer boundary" and one or more clock-wise
% "inner boundaries" around "holes".
% POLYUNI Union of 2 polygons.
% [XO,YO] = POLYINT(X1,Y1,X2,Y2) Calculates polygon(s) P
% which is (are) union of polygons P1 and P2 with coordinates
% X1, Y1 and X2, Y2.
% The resulting polygon(s) is a set of all points which belong
% either to P1 or to P2: P = P1 | P2.
% The input polygons must be non-self-intersecting and
% simply connected.
%
% If polygons P1, P2 are not intersecting, returns
% coordinates of the both polygons separated by NaN.
% If both P1 and P2 are convex, their boundaries can have no
% more than 2 intersections. The result is also a convex
% polygon.
% If the result P is not simply connected (such as a polygon
% with a "hole") the resulting contour consist of counter-
% clockwise "outer boundary" and one or more clock-wise
% "inner boundaries" around "holes".
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/25/95
% Call POLYBOOL with flag=2 ..........
[xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 2);
r = Polygon([xo(:) yo(:)]');
end
function r = xor(p, q)
%Polygon.xor Exclusive or of polygons
%
% I = P.union(Q) is a Polygon representing the
% union of polygons P and Q.
%
% Notes::
% - If these polygons are not intersecting, returns a polygon with
% vertices of both polygons separated by NaNs.
% - If the result P is not simply connected (such as a polygon
% with a "hole") the resulting contour consist of counter-
% clockwise "outer boundary" and one or more clock-wise
% "inner boundaries" around "holes".
% POLYXOR Exclusive OR of 2 polygons.
% [XO,YO] = POLYXOR(X1,Y1,X2,Y2) Calculates polygon(s) P
% of difference of polygons P1 and P1 with coordinates
% X1, Y1 and X2, Y2.
% The resulting polygon(s) is a set of all points which belong
% either to P1 or to P2 but not to both:
% P = (P1 & ~P2) | (P2 & ~P1).
% The input polygons must be non-self-intersecting and
% simply connected.
%
% If polygons P1, P2 are not intersecting, returns
% coordinates of the both polygons separated by NaN.
% If the result P is not simply connected or consists of several
% polygons, resulting boundary consists of concatenated
% coordinates of these polygons, separated by NaN.
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/25/95
% Call POLYBOOL twice with flag=3
[xx,yy,ind] = polybool(p.x, p.y, q.x, q.y, 3);
xo = [xx; NaN]; yo = [yy; NaN];
[xx,yy,ind] = polybool(q.x, q.y, p.x, p.y, 3);
xo = [xo; xx]; yo = [yo; yy];
r = Polygon([xo(:) yo(:)]');
end
end
end
function [is,in,un] = interval(x1,x2)
% Intersection and union of 2 intervals.
% [IS,IN,UN] = INTERVAL(X1,X2) calculates pair-wise
% intersection IN and union UN of N pairs of
% intervals with coordinates X1 and X2 (both are
% 2 by N vectors). Returns 1 by N boolean vector IS
% equal to 1 if intervals have non-empty intersection
% and 0 if they don't.
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 08/24/95
% Handle input ...........................
if nargin==0, help interval, return, end
if nargin==1
un = x1;
else
un = [x1; x2];
end
[in,un] = sort(un); % Sort both intervals together
un = un(1:2,:)-1;
is = sum(floor(un/2)); % Check for [0 0 1 1] or [1 1 0 0]
is = (is==1);
ii = find(in(2,:)==in(3,:));
is(ii) = .5*ones(size(ii));
% Extract intersection and union from sorted coordinates
if nargout>1
un = in([1 4],:);
in = in(2:3,:);
in(:,~is) = flipud(in(:,~is));
end
end
function [is,S] = iscross(x1,y1,x2,y2,tol)
% ISCROSS Finds whether pairs of lines cross each other
% [IS,S] = ISCROSS(X1,Y1,X2,Y2) where arguments X1, Y1,
% X2, Y2 are all 2 by N matrices are coordinates of
% ends of the pairs of line segments.
% Returns vector IS (1 by N) consisting of ones if
% corresponding pairs cross each other, zeros if they
% don't and .5 if an end of one line segment lies on
% another segment.
% Also returns a matrix S (4 by N) with each row
% consisting of cross products (double areas of
% corresponding triangles) built on the following points:
% (X2(1,:),Y2(1,:)),(X1(1,:),Y1(1,:)),(X2(2,:),Y2(2,:)),
% (X2(1,:),Y2(1,:)),(X1(2,:),Y1(2,:)),(X2(2,:),Y2(2,:))
% (X1(1,:),Y1(1,:)),(X2(1,:),Y2(1,:)),(X1(2,:),Y1(2,:))
% (X1(1,:),Y1(1,:)),(X2(2,:),Y2(2,:)),(X1(2,:),Y1(2,:))
% The signs of these 4 areas can be used to determine
% whether these lines and their continuations cross each
% other.
% [IS,S] = ISCROSS(X1,Y1,X2,Y2,TOL) uses tolerance TOL
% for detecting the crossings (default is 0).
% Copyright (c) 1995 by Kirill K. Pankratov
% kirill@plume.mit.edu
% 08/14/94, 05/18/95, 08/25/95
% Defaults and parameters .......................
tol_dflt = 0; % Tolerance for area calculation
is_chk = 1; % Check input arguments
% Handle input ..................................
if nargin==0, help iscross, return, end
if nargin<4 % Check if all 4 entered
error(' Not enough input arguments')
end
if nargin<5, tol = tol_dflt; end
if tol < 0, is_chk = 0; tol = 0; end
% Check the format of arguments .................
if is_chk
[x1,y1,x2,y2] = linechk(x1,y1,x2,y2);
end
len = size(x1,2);
o2 = ones(2,1);
% Find if the ranges of pairs of segments intersect
[isx,S,A] = interval(x1,x2);
scx = diff(A);
[isy,S,A] = interval(y1,y2);
scy = diff(A);
is = isx & isy;
% If S values are not needed, extract only those pairs
% which have intersecting ranges ..............
if nargout < 2
isx = find(is); % Indices of pairs to be checked
% further
x1 = x1(:,isx);
x2 = x2(:,isx);
y1 = y1(:,isx);
y2 = y2(:,isx);
is = is(isx);
if isempty(is), is = zeros(1,len); return, end
scx = scx(isx);
scy = scy(isx);
end
% Rescale by ranges ...........................
x1 = x1.*scx(o2,:);
x2 = x2.*scx(o2,:);
y1 = y1.*scy(o2,:);
y2 = y2.*scy(o2,:);
% Calculate areas .............................
S = zeros(4,length(scx));
S(1,:) = (x2(1,:)-x1(1,:)).*(y2(2,:)-y1(1,:));
S(1,:) = S(1,:)-(x2(2,:)-x1(1,:)).*(y2(1,:)-y1(1,:));
S(2,:) = (x2(1,:)-x1(2,:)).*(y2(2,:)-y1(2,:));
S(2,:) = S(2,:)-(x2(2,:)-x1(2,:)).*(y2(1,:)-y1(2,:));
S(3,:) = (x1(1,:)-x2(1,:)).*(y1(2,:)-y2(1,:));
S(3,:) = S(3,:)-(x1(2,:)-x2(1,:)).*(y1(1,:)-y2(1,:));
S(4,:) = (x1(1,:)-x2(2,:)).*(y1(2,:)-y2(2,:));
S(4,:) = S(4,:)-(x1(2,:)-x2(2,:)).*(y1(1,:)-y2(2,:));
% Find if they cross each other ...............
is = (S(1,:).*S(2,:)<=0)&(S(3,:).*S(4,:)<=0);
% Find very close to intersection
isy = min(abs(S));
ii = find(isy<=tol & is);
is(ii) = .5*ones(size(ii));
% Output
if nargout < 2
isy = zeros(1,len);
isy(isx) = is;
is = isy;
else
isy = scx.*scy;
ii = find(~isy);
isy(ii) = ones(size(ii));
S = S./isy(ones(4,1),:);
end
end
function [xo,yo,ind] = polybool(x1,y1,x2,y2,flag)
% [XO,YO] = POLYBOOL(X1,Y1,X2,Y2,FLAG)
% calulates results of Boolean operations on
% a pair of polygons.
% FLAG Specifies the type of the operation:
% 1 - Intersection (P1 & P2)
% 2 - Union (P1 | P2)
% 3 - Difference (P1 & ~P2)
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/25/95, 09/07/95
% This program calls the following functions:
% AREA, ISINTPL, ISCROSS, INTSECL.
% Algorithm:
% 1. Check boundary contour directions (area).
% For intersection and union make all
% counter-clockwise. For difference make the second
% contour clock-wise.
% 2. Calculate matrix of intersections (function ISINTPL).
% Quick exit if no intersections.
% 3. For intersecting segments calculate intersection
% coordinates (function INTSECL).
% 4. Sort intersections along both contours.
% 5. Calculate sign of cross-product between intersectiong
% segments. This will give which contour goes "in" and
% "out" at intersections.
%
% 6. Start with first intersection:
% Determine direction to go ("in" for intersection,
% "out" for union).
% Move until next intersection, switch polygons at each
% intersection until coming to the initial point.
% If not all intersections are encountered, the
% resulting polygon is disjoint. Separate output
% coordinates by NaN and repeat procedure until all
% intersections are counted.
% Default for flag
flag_dflt = 1; % 1- intersec., 2-union, 3 - diff.
% Handle input
if nargin==0, help polybool, return, end
if nargin < 4
error(' Not enough input arguments')
end
if nargin<5, flag = flag_dflt; end
x1 = x1(:); y1 = y1(:);
x2 = x2(:); y2 = y2(:);
l1 = length(x1);
l2 = length(x2);
% Check areas and reverse if negative
nn1 = area(x1,y1);
if nn1<0, x1 = flipud(x1); y1 = flipud(y1); end
nn2 = area(x2,y2);
if (nn2<0 & flag<3) | (nn2>0 & flag==3)
x2 = flipud(x2); y2 = flipud(y2);
end
% If both polygons are identical ........
if l1==l2
if all(x1==x2) & all(y1==y2)
if flag<3, xo = x1; yo = y1; ind = 1:l1;
else, xo = []; yo = []; ind = []; end
return
end
end
% Calculate matrix of intersections .....
[is,C] = isintpl(x1,y1,x2,y2);
is = any(any(C));
% Quick exit if no intersections ........
if ~is
if flag==1 % Intersection
xo=[]; yo = [];
elseif flag==2 % Union
xo = [x1; nan; x2];
yo = [y1; nan; y2];
elseif flag==3 % Difference
xo = x1; yo = y1;
end
return
end
% Mark intersections with unique numbers
i1 = find(C);
ni = length(i1);
C(i1) = 1:ni;
% Close polygon contours
x1 = [x1; x1(1)]; y1 = [y1; y1(1)];
x2 = [x2; x2(1)]; y2 = [y2; y2(1)];
l1 = length(x1); l2 = length(x2);
% Calculate intersections themselves
[i1,i2,id] = find(C);
xs1 = [x1(i1) x1(i1+1)]'; ys1 = [y1(i1) y1(i1+1)]';
xs2 = [x2(i2) x2(i2+1)]'; ys2 = [y2(i2) y2(i2+1)]';
% Call INTSECL ............................
[xint,yint] = intsecl(xs1,ys1,xs2,ys2);
% For sements belonging to the same line
% find interval of intersection ...........
ii = find(xint==inf);
if ~isempty(ii)
[is,inx] = interval(xs1(:,ii),xs2(:,ii));
[is,iny] = interval(ys1(:,ii),ys2(:,ii));
xint(ii) = mean(inx);
yint(ii) = mean(iny);
end
% Coordinate differences of intersecting segments
xs1 = diff(xs1); ys1 = diff(ys1);
xs2 = diff(xs2); ys2 = diff(ys2);
% Calculate cross-products
cp = xs1.*ys2-xs2.*ys1;
cp = cp>0;
if flag==2, cp=~cp; end % Reverse if union
cp(ii) = 2*ones(size(ii));
% Sort intersections along the contours
ind = (xint-x1(i1)').^2+(yint-y1(i1)').^2;
ind = ind./(xs1.^2+ys1.^2);
cnd = min(ind(ind>0));
ind = ind+i1'+i2'/(ni+1)*cnd*0;
[xo,ii] = sort(ind);
xs1 = id(ii);
[xo,ind] = sort(xs1);
ind = rem(ind,ni)+1;
xs1 = xs1(ind);
ind = (xint-x2(i2)').^2+(yint-y2(i2)').^2;
ind = ind./(xs2.^2+ys2.^2);
cnd = min(ind(ind>0));
[xo,ii] = sort(i2'+ind+i1'/(ni+1)*cnd*0);
xs2 = id(ii);
[xo,ind] = sort(xs2);
ind = rem(ind,ni)+1;
xs2 = xs2(ind);
% Combine coordinates in one vector
x1 = [x1; x2]; y1 = [y1; y2];
% Find max. possible length of a chain
xo = find(any(C'));
xo = diff([xo xo(1)+l1]);
mlen(1) = max(xo);
xo = find(any(C));
xo = diff([xo xo(1)+l2]);
mlen(2) = max(xo);
% Check if multiple intersections in one segment
xo = diff([i1 i2]);
is_1 = ~all(all(xo));
% Begin counting intersections *********************
% Initialization ..................
int = zeros(size(xint));
nn = 1; % First intersection
nn1 = i1(nn); nn2 = i2(nn);
b = cp(nn);
is2 = b==2;
xo = []; yo = []; ind = [];
closed = 0;
% Proceed until all intersections are counted
while ~closed % begin counting `````````````````````0
% If contour closes, find new starting point
if int(nn) & ~all(int)
ii = find(int);
C(id(ii)) = zeros(size(ii));
nn = min(find(~int)); % Next intersection
nn1 = i1(nn);
nn2 = i2(nn);
xo = [xo; nan]; % Separate by NaN
yo = [yo; nan];
ind = [ind; nan];
% Choose direction ......
b = cp(nn);
end
% Add current intersection ......
xo = [xo; xint(nn)];
yo = [yo; yint(nn)];
ind = [ind; 0];
int(nn) = 1;
closed = all(int);
% Find next segment
% Indices for next intersection
if ~b, nn = xs1(nn);
else, nn = xs2(nn);
end
if ~b, pt0 = nn1; else, pt0 = nn2; end
nn1 = i1(nn);
nn2 = i2(nn);
if b, pt = nn2; else, pt = nn1; end
if b, pt0 = pt0+l1; pt = pt+l1; end
ii = (pt0+1:pt);
% Go through the beginning ..............
cnd = pt<pt0 | (pt==pt0 & is_1 & flag>1);
if cnd
if ~b, ii = [pt0+1:l1 1:pt];
else, ii = [pt0+1:l1+l2 l1+1:pt];
end
end
len = length(ii);
cnd = b & len>mlen(2);
cnd = cnd | (~b & len>mlen(1));
if is2 | cnd, ii=[]; end
% Add new segment
xo = [xo; x1(ii)];
yo = [yo; y1(ii)];
ind = [ind; ii'];
% Switch direction
if cp(nn)==2, b = ~b; is2 = 1;
else, b = cp(nn); is2 = 0;
end
end % End while (all intersections) '''''''''''''''0
% Remove coincident successive points
ii = find(~diff(xo) & ~diff(yo));
xo(ii) = []; yo(ii) = []; ind(ii) = [];
% Remove points which are
ii = find(isnan(xo));
if ~isempty(ii)
i2 = ones(size(xo));
ii = [ii; length(xo)+1];
i1 = find(diff(ii)==3);
i1 = ii(i1);
i1 = [i1; i1+1; i1+2];
i2(i1) = zeros(size(i1));
i1 = find(diff(ii)==2);
i1 = ii(i1);
i1 = [i1; i1+1];
i2(i1) = zeros(size(i1));
xo = xo(i2); yo = yo(i2); ind = ind(i2);
end
end
function [xo,yo] = intsecpl(xv,yv,xl,yl,trace)
% INTSECPL Intersection of a polygon and a line.
% [XI,YI] = INTSECPL(XV,YV,XL,YL) calculates
% intersections XI, YI of a polygon with vertices XV,
% YV and a line specified by pairs of end coordinates
% XL = [XL0 XL1], YL = [YL0 YL1]. Line is assumed to
% continue beyond the range of end points.
% INTSECPL(XV,YV,[A B]) uses another specification for
% a line: Y = A*X+B.
%
% If a line does not intersect polygon, returns empty
% XI, YI.
% For convex polygon maximum number of intersections is
% 2, for non-convex polygons multiple intersections are
% possible.
%
% INTSECPL(XV,YV,XL,YL) by itself or
% [XI,YI] = INTSECPL(XV,YV,XL,YL,1) plots polygon,
% a line segment and intersection segment(s)
% (part(s) of the same line inside the polygon).
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/25/95, 08/27/95, 09/27/95
% Calls ISCROSS, INTSECL programs.
% Defaults and parameters .................................
tol = 1e-14; % Tolerance
marg = tol; % Margins for polygon frame
is_ab = 0; % Default A*X+B mode
% Handle input ............................................
if nargin==0, help intsecpl, return, end
if nargin < 3
error(' Not enough input arguments')
end
if nargin<5, trace = 0; end
if nargin==4 % Check if 4-th arg is trace
if max(size(yl))==1, trace = yl; is_ab = 1; end
end
if nargin==3, is_ab = 1; end
trace = trace | nargin<2;
if length(xv)~=length(yv)
error(' Vectors X, Y must have the same size')
end
% Auxillary ...........
xv = [xv(:); xv(1)];
yv = [yv(:); yv(1)];
ii = find(abs(diff(xv))<tol & abs(diff(yv))<tol);
xv(ii) = []; yv(ii) = [];
nv = length(xv);
ov = ones(nv-1,1);
% Polygon frame
lim = [min(xv)-marg max(xv)+marg min(yv)-marg max(yv)+marg];
% Estimate for diameter
d = sqrt((lim(2)-lim(1)).^2+(lim(4)-lim(3)).^2);
% Form line segment depending on how line is specified
if is_ab % A*X+B mode ...................
xl = xl(:);
if length(xl)<2
error(' Line is specified by at least two parameters')
end
a = xl(1); b = xl(2);
xl = [lim(1)-1 lim(2)+1];
yl = a*xl+b;
else % [X1 X2],[Y1 Y2] mode ..........
x0 = (xl(1)+xl(2))/2;
y0 = (yl(1)+yl(2))/2;
dx = xl(2)-x0;
dy = yl(2)-y0;
dl = max(abs([lim(1:2)-x0 lim(3:4)-y0]));
d = max(d,dl);
dl = sqrt(dx.^2+dy.^2);
dl = max(d,d/dl);
dx = dx*dl; dy = dy*dl;
xl = [x0-dx x0+dx];
yl = [y0-dy y0+dy];
end
% Find intersecting segments ..............................
is = iscross([xv(1:nv-1) xv(2:nv)]',[yv(1:nv-1) yv(2:nv)]',...
xl(ov,:)',yl(ov,:)',0);
% Quick exit if no intersections .........................
if ~any(is)
if trace
% Intersection with polygon frame
[xl,yl] = intsecpl(lim([1 2 2 1]),lim([3 3 4 4]),xl,yl);
plot(xv,yv,xl,yl) % Plotting itself
end
xo = []; yo = [];
return
end
% For segments touching the line (is==.5) find whether pairs of
% successive segments are on the same side
ii = find(is==.5)';
if ii~=[]
xo = [ii-1 ii+1];
xo = xo+(nv-1)*(xo<1);
yo = iscross([xv(xo(:,1)) xv(xo(:,2))]',[yv(xo(:,1)) yv(xo(:,2))]',...
xl,yl,tol);
ii = ii(find(yo==1));
is(ii) = zeros(size(ii));
end
% Calculate intersection coordinates ......................
ii = find(is);
oi = ones(size(ii));
[xo,yo] = intsecl([xv(ii) xv(ii+1)]',[yv(ii) yv(ii+1)]',...
xl(oi,:)',yl(oi,:)');
dx = find(~finite(xo));
xo(dx) = []; yo(dx) = [];
ii(dx) = []; oi(dx) = [];
% Sort intersections along the line ..........
xo = xo(:); yo = yo(:);
if any(diff(xo))
[xo,ii] = sort(xo);
yo = yo(ii);
else
[yo,ii] = sort(yo);
xo = xo(ii);
end
% Exclude repeated points (degenerate cases)
% ///////// It seems that this is not needed, figure this
% later ///////////////////
if length(ii)>1 & 0 % Do not execute
xx = [xo yo];
yy = diff(xx)';
ii = [1 find(any(abs(yy)>tol))+1];
xo = xx(ii,1); yo = xx(ii,2);
oi = ones(size(xo));
end
% Plotting ................................................
if trace
oi(3:2:length(oi)) = oi(3:2:length(oi))+1;
oi = cumsum(oi);
len = max(oi);
xp = nan*ones(len,1); yp = xp;
xp(oi) = xo;
yp(oi) = yo;
% Intersection with polygon frame
[xl,yl] = intsecpl(lim([1 2 2 1]),lim([3 3 4 4]),xl,yl);
plot(xv,yv,xl,yl,xp,yp) % Plotting itself
end
end
function [is,S] = isintpl(x1,y1,x2,y2)
% ISINTPL Check for intersection of polygons.
% [IS,S] = ISINTPL(X1,Y1,X2,Y2) Accepts coordinates
% X1,Y1 and X2, Y2 of two polygons. Returns scalar
% IS equal to 1 if these polygons intersect each other
% and 0 if they do not.
% Also returns Boolean matrix S of the size length(X1)
% by length(X2) so that {ij} element of which is 1 if
% line segments i to i+1 of the first polygon and
% j to j+1 of the second polygon intersect each other,
% 0 if they don't and .5 if they "touch" each other.
% Copyright (c) 1995 by Kirill K. Pankratov,
% kirill@plume.mit.edu.
% 06/20/95, 08/25/95
% Handle input ...................................
if nargin==0, help isintpl, return, end
if nargin<4
error(' Not enough input arguments ')
end
% Make column vectors and check sizes ............
x1 = x1(:); y1 = y1(:);
x2 = x2(:); y2 = y2(:);
l1 = length(x1);
l2 = length(x2);
if length(y1)~=l1 | length(y2)~=l2
error('(X1,Y1) and (X2,Y2) must pair-wise have the same length.')
end
% Quick exit if empty
if l1<1 | l2<1, is = []; S = []; return, end
% Check if their ranges are intersecting .........
lim1 = [min(x1) max(x1)]';
lim2 = [min(x2) max(x2)]';
isx = interval(lim1,lim2); % X-ranges
lim1 = [min(y1) max(y1)]';
lim2 = [min(y2) max(y2)]';
isy = interval(lim1,lim2); % Y-ranges
is = isx & isy;
S = zeros(l2,l1);
if ~is, return, end % Early exit if disparate limits
% Indexing .......................................
[i11,i12] = meshgrid(1:l1,1:l2);
[i21,i22] = meshgrid([2:l1 1],[2:l2 1]);
i11 = i11(:); i12 = i12(:);
i21 = i21(:); i22 = i22(:);
% Calculate matrix of intersections ..............
S(:) = iscross([x1(i11) x1(i21)]',[y1(i11) y1(i21)]',...
[x2(i12) x2(i22)]',[y2(i12) y2(i22)]')';
S = S';
is = any(any(S));
end
function [xo,yo] = intsecl(x1,y1,x2,y2,tol)
% INTSECL Intersection coordinates of two line segments.
% [XI,YI] = INTSECL(X1,Y1,X2,Y2) where all 4
% arguments are 2 by N matrices with coordinates
% of ends of N pairs of line segments (so that
% the command such as PLOT(X1,Y1,X2,Y2) will plot
% these pairs of lines).
% Returns 1 by N vectors XI and YI consisting of
% coordinates of intersection points of each of N
% pairs of lines.
%
% Special cases:
% When a line segment is degenerate into a point
% and does not lie on line through the other segment
% of a pair returns XI=NaN while YI has the following
% values: 1 - when the first segment in a pair is
% degenerate, 2 - second segment, 0 - both segments
% are degenerate.
% When a pair of line segments is parallel, returns
% XI = Inf while YI is 1 for coincident lines,
% 0 - for parallel non-coincident ones.
% INTSECL(X1,Y1,X2,Y2,TOL) also specifies tolerance
% in detecting coincident points in different line
% segments.
% Copyright (c) 1995 by Kirill K. Pankratov
% kirill@plume.mit.edu
% 04/15/94, 08/14/94, 05/10/95, 08/23/95
% Defaults and parameters .........................
tol_dflt = 0; % Tolerance for coincident points
is_chk = 1; % Check input arguments
% Handle input ....................................
if nargin==0, help intsecl, return, end
if nargin<4 % Check if all 4 entered
error(' Not enough input arguments')
end
if nargin<5, tol = tol_dflt; end
if tol < 0, is_chk = 0; tol = 0; end
% Check the format of arguments .......
if is_chk
[x1,y1,x2,y2] = linechk(x1,y1,x2,y2);
end
% Auxillary
o2 = ones(2,1);
i_pt1 = []; i_pt2 = []; i_pt12 = [];
% Make first points origins ...........
xo = x1(1,:);
yo = y1(1,:);
x2 = x2-xo(o2,:);
y2 = y2-yo(o2,:);
% Differences of first segments .......
a = x1(2,:)-x1(1,:);
b = y1(2,:)-y1(1,:);
s = sqrt(a.^2+b.^2); % Lengths of first segments
i_pt1 = find(~s);
s(i_pt1) = ones(size(i_pt1));
rr = rand(size(i_pt1));
a(i_pt1) = cos(rr);
b(i_pt1) = sin(rr);
% Normalize by length .................
a = a./s; b = b./s;
% Rotate coordinates of the second segment
tmp = x2.*a(o2,:)+y2.*b(o2,:);
y2 = -x2.*b(o2,:)+y2.*a(o2,:);
x2 = tmp;
% Calculate differences in second segments
s = x2(2,:)-x2(1,:);
tmp = y2(2,:)-y2(1,:);
cc = tmp(i_pt1);
% Find some degenerate cases .......................
% Find zeros in differences
izy2 = find(~tmp);
tmp(izy2) = ones(size(izy2));
% Find degenerate and parallel segments
bool = ~s(izy2);
i_par = izy2(~bool);
i_pt2 = izy2(bool);
bool = abs(y2(1,i_pt2))<=tol;
i_pt2_off = i_pt2(~bool);
i_pt2_on = i_pt2(bool);
if ~isempty(i_par)
bool = abs(y2(1,i_par))<=tol;
i_par_off = i_par(~bool);
i_par_on = i_par(bool);
end
% Calculate intercept with rotated x-axis ..........
tmp = s./tmp; % Slope
tmp = x2(1,:)-y2(1,:).*tmp;
% Rotate and translate back to original coordinates
xo = tmp.*a+xo;
yo = tmp.*b+yo;
% Mark special cases ...................................
% First segments are degenerate to points
if ~isempty(i_pt1)
bool = ~s(i_pt1) & ~cc;
i_pt12 = i_pt1(bool);
i_pt1 = i_pt1(~bool);
bool = abs(tmp(i_pt1))<=tol;
i_pt1_on = i_pt1(bool);
i_pt1_off = i_pt1(~bool);
xo(i_pt1_on) = x1(1,i_pt1_on);
yo(i_pt1_on) = y1(1,i_pt1_on);
oo = ones(size(i_pt1_off));
xo(i_pt1_off) = nan*oo;
yo(i_pt1_off) = oo;
end
% Second segments are degenerate to points ...
if ~isempty(i_pt2)
oo = ones(size(i_pt2_off));
xo(i_pt2_off) = nan*oo;
yo(i_pt2_off) = 2*oo;
end
% Both segments are degenerate ...............
if ~isempty(i_pt12)
bool = x1(i_pt12)==xo(i_pt12);
i_pt12_on = i_pt12(bool);
i_pt12_off = i_pt12(~bool);
xo(i_pt12_on) = x1(1,i_pt12_on);
yo(i_pt12_on) = y1(1,i_pt12_on);
oo = ones(size(i_pt12_off));
xo(i_pt12_off) = nan*oo;
yo(i_pt12_off) = 0*oo;
end
% Parallel segments .........................
if ~isempty(i_par)
oo = ones(size(i_par_on));
xo(i_par_on) = inf*oo;
yo(i_par_on) = oo;
oo = ones(size(i_par_off));
xo(i_par_off) = inf*oo;
yo(i_par_off) = 0*oo;
end
end