function [rpv, crpv, spv, xppv, yppv, jppv, sv] = poly_poly_proj(xp, yp, xv, yv)
%poly_poly_proj Project a planar polyline on another polyline.
%*******************************************************************************
% function: poly_poly_proj
% Description: Compute the distances (cross-ranges) from a set of points
% P <p(1), p(2), ..., p(m)> on a plane to the planar polyline
% V <v(1), v(2), ..., v(n)>, and the distances (ranges) from the
% projections of P on V to the first point of a segment containing
% the projected point. Planar polyline V is defined as a set of n-1
% segments connecting n ordered vertices v(1), v(2), ..., v(n) on a
% plane.
% In case when the shortest distance is to the vertex of the
% polyline (see p(2) in the picture below), the cross range
% cr(2) will be the distance to the vertex and the range will be
% the length of this segment (segment # 3)
%
% p(1)
% o p(m)
% | o
% cr(1)-> | p(k) /
% | o / <- cr(m)
% v(1) v(2) | cr(k)-> | /
% x---x | | v(n)/
% \ | x-->|--------x <- r(m) = 0
% \ | v(3) / r(k)
% v(3) x->|--------x r(2)
% |->| \ <-cr(2)
% r(1) \
% o p(2)
% Input:
% xp - x coordinates of the first polyline (vector of length m)
% yp - y coordinates of the first polyline (vector of length m)
% xv - x coordinates of the second polyline (vector of length n)
% yv - y coordinates of the second polyline (vector of length n)
% Output:
% rpv - vector (of length m) of distances (ranges) from the projections on
% the second polyline to the beginnings of the segments containing
% these projections
% crpv - vector (of length m) of distances (cross-ranges) from points of the
% first polyline (P) to a second polyline (V), defined as a minimal
% distance from each point of the first polyline to any segment of a
% second polyline. Note that cr is a signed value! The sign of
% cross range is determined using a local coordinate system, where
% X axis is along the rib k, from point v(k) to v(k+1), Z axis is
% out of plane, and Y axis completes the right-hand system. The
% sign of projected point's p(j) y coordinate will determine the
% cross-range sign for this point.
% y
% ^
% | o p(j)
% | /|
% | / | <- cr > 0
% |/ |
% x---|-------x--------------> x
% v(k) v(k+1)
% spv - total projected path length. Can be negative, if P and V are in the
% opposite directions
% xppv - vector of x coordinates of the first polyline projections on the
% second polyline
% yppv - vector of y coordinates of the first polyline projections on the
% second polyline
% jppv - indices of the first points in segments containing the projections
% (vector of length m). In the abovementioned example, this vector
% should be jpv = [2, 4, 5, 6]. Note that the shortest distances
% of points p(1), p(2) and p(4) are not to the second polyline's
% ribs, but to the verices.
% Routines:
% Revision history:
% 12/05/2009 - added dimension parameter to cross function-Michael Yoshpe
% 26/11/2008 - created by Michael Yoshpe
% Remarks:
% Based on routine p_poly_dist that computed the distance from point to
% polygon
% Usage example:
% % define the two polylines, P and V (P will be projected on V)
% xp = [1 3 5 7 9 10 12];
% yp = [1 2 -2 -3 4 1 -1];
% xv = [-2 2 4 6 11];
% yv = [-1 3 2 4 -1];
% np = length(xp);
%
% % run poly_poly_proj
% [rpv, crpv, spv, xppv, yppv, jppv, sv] = poly_poly_proj(xp, yp, xv, yv);
%
% % plot the polylines and the projected points (xppv, yppv)
% figure;
% plot(xp, yp, 'bo-', xv, yv, 'gs-', xppv, yppv, 'rx');
% legend('P polyline', 'V polyline', 'projections of P on V');
% hold on;
% grid on;
% axis equal;
%
% % plot the projection dashed lines and the cross-range annotations
% for j=1:np,
% plot([xp(j) xppv(j)], [yp(j) yppv(j)], 'k--');
% text(0.5*(xp(j)+xppv(j))+0.1, 0.5*(yp(j)+yppv(j))+0.1,...
% ['crpv(' num2str(j) ')']);
% end
%*******************************************************************************
% linear parameters of segments that connect the vertices
xv = xv(:);
yv = yv(:);
xp = xp(:);
yp = yp(:);
A = -diff(yv);
B = diff(xv);
C = yv(2:end).*xv(1:end-1) - xv(2:end).*yv(1:end-1);
ns = length(A); % number of segments in second polyline
% some sanity checks
if(ns==0)
error('Second polyline must have at least one segment (2 points)');
end
if(length(xv) ~= (ns + 1))
error('xv and yv must have the same numbber of points!');
end
idx_v0 = find((A==0) & (B==0));
if(~isempty(idx_v0))
error(['No zero length segments are allowed in second polyline! '...
'The following consecutive points are identical: ' num2str(idx_v0)]);
end
np = length(xp); % number of points defining the first polyline
if(length(yp) ~= np)
error('xp and yp must have the same numbber of points!');
end
% find the projection of each point in (xp,yp) on each rib
AB = 1./(A.^2 + B.^2);
vv = (A(:)*xp(:)'+B(:)*yp(:)'+C(:)*ones(1,np));
xpv = ones(ns,1)*xp(:)' - (A(:).*AB(:))*ones(1,np).*vv;
ypv = ones(ns,1)*yp(:)' - (B(:).*AB(:))*ones(1,np).*vv;
% find all cases where projected points are inside the segment
idx_x = (((xpv>=repmat(xv(1:end-1),1,np)) &(xpv<=repmat(xv(2:end),1,np))) |...
((xpv>=repmat(xv(2:end),1,np)) &(xpv<=repmat(xv(1:end-1),1,np))));
idx_y = (((ypv>=repmat(yv(1:end-1),1,np)) &(ypv<=repmat(yv(2:end),1,np))) |...
((ypv>=repmat(yv(2:end),1,np)) & (ypv<=repmat(yv(1:end-1),1,np))));
idx = idx_x & idx_y;
% distance from point (x,y) to the vertices
dpv = sqrt((repmat(xv, 1, np) - repmat(xp', ns+1,1)).^2 + ...
(repmat(yv, 1, np) - repmat(yp', ns+1,1)).^2);
% compute the length of each segment and the total length of V at each point
vds = sqrt((xv(2:end) - xv(1:end-1)).^2 + (yv(2:end) - yv(1:end-1)).^2);
vds1 = [0; vds]; % segment length at each point of V
sv = [0; cumsum(vds)]; % total length of V at each point
if(~any(idx)) % all projections are outside of the second polyline's segments
[crpv, jppv] = min(dpv);
rpv = vds1(jppv); % range of projected points in this case is simply the
% segment length
xppv = xv(jppv); % the the projection coordinates are the vertex
% coordinates of V
yppv = yv(jppv); % the the projection coordinates are the vertex coordinates
% of V
else
% distance from points (xp,yp) to the projection on ribs
xpv_mask = NaN(size(xpv)); % build the matrix the size of xpv and fill it
% with NaNs to serve as a mask
xpv_mask(idx) = xpv(idx); % now only the projections with x coordinates
% INSIDE the ribs are NOT NaNs
ypv_mask = NaN(size(ypv)); % build the matrix the size of xpv and fill it
% with NaNs to serve as a mask
ypv_mask(idx) = ypv(idx); % now only the projections with y coordinates
% INSIDE the ribs are NOT NaNs
dpp = sqrt((xpv_mask-ones(ns,1)*xp(:)').^2 +(ypv_mask-ones(ns,1)*yp(:)').^2);
[min_dpp, jmin_dpp] = min(dpp); % minimum distances to ribs (if exist) and
% ribs indices
[min_dpv, jmin_dpv] = min(dpv); % minimum distances to vertices and vertex
% indices
jout = find(isnan(min_dpp)); % find projections outside of any ribs
% in case there were projections outside of any ribs, the corresponding
% minimum values should be the minimum values of distances to verices
if(~isempty(jout))
jppv(jout) = jmin_dpv(jout);
rpv(jout) = 0; % if the shortest distance is to the vertex, the projection
% is at 0 distance from that vertex
crpv(jout) = min_dpv(jout);
xppv(jout) = xv(jppv(jout));
yppv(jout) = yv(jppv(jout));
end
% find all the cases where distances to the projections on ribs are LESS or
% equal than distances to vertices for the same points of first polyline
jmin1 = find(min_dpp <= min_dpv);
jppv(jmin1) = jmin_dpp(jmin1);
% build a linear index into xpv_mask and ypv_mask from row-column pairs
idx_ppv1 = sub2ind(size(xpv_mask), jppv(jmin1), jmin1);
xppv(jmin1) = xpv_mask(idx_ppv1(:));
yppv(jmin1) = ypv_mask(idx_ppv1(:));
rpv(jmin1) = sqrt((xv(jppv(jmin1)') - xppv(jmin1)').^2 + ...
(yv(jppv(jmin1)') - yppv(jmin1)').^2);
crpv(jmin1) = min_dpp(jmin1);
% find all the cases where the distances to the vertices are LESS than
% distances to projections for the same points of first polyline
jmin2 = find(min_dpv < min_dpp);
jppv(jmin2) = jmin_dpv(jmin2);
xppv(jmin2) = xv(jppv(jmin2));
yppv(jmin2) = yv(jppv(jmin2));
rpv(jmin2) =0; % if the shortest distance is to the vertex, the projection is
% at 0 distance from that vertex
crpv(jmin2) = min_dpv(jmin2);
end
% Finding the sign of cross-range cr. If the point of the first polyline is
% "to the left" from the second polyline, the cr for this point is
% positive, otherwise it's negative
% special treatment for the projected point which is the last vertex from
% second polyline
idx_last_point = find(jppv == ns+1);
idx_not_last_point = find(jppv ~= ns+1);
% vector components along ribs of V that include projections of P
uxv1 = xv(jppv(idx_not_last_point)+1) - xv(jppv(idx_not_last_point));
uyv1 = yv(jppv(idx_not_last_point)+1) - yv(jppv(idx_not_last_point));
uxp1 = xp(idx_not_last_point) - xv(jppv(idx_not_last_point));
uyp1 = yp(idx_not_last_point) - yv(jppv(idx_not_last_point));
if(~isempty(idx_last_point)) % there are points in P that are "beyond" the last
% vertex
nlast = length(idx_last_point); % number of points in P beyond the last
% vertex
uxv_last = (xv(end) - xv(end-1))*ones(nlast,1);
uyv_last = (yv(end) - yv(end-1))*ones(nlast,1);
uxp_last = xp(idx_last_point) - xv(end);
uyp_last = yp(idx_last_point) - yv(end);
uxv = [uxv1; uxv_last];
uyv = [uyv1; uyv_last];
uxp = [uxp1; uxp_last];
uyp = [uyp1; uyp_last];
else
uxv = uxv1;
uyv = uyv1;
uxp = xp(idx_not_last_point) - xv(jppv(idx_not_last_point));
uyp = yp(idx_not_last_point) - yv(jppv(idx_not_last_point));
end
% now find the sign of cross range (positive if point of P is "to the left"
% of V, negative if to the right, zero if on V)
ap = [uxp(:) uyp(:) zeros(np,1)];
av = [uxv(:) uyv(:) zeros(np,1)];
az = cross(av, ap, 2);
% Guard against the rare case when the points of P are located on the line,
% containing the rib of V, but not inside the rib (this case is treated as
% "distance to vertex"), but the sign function might zero it out.
idx_non0 = (az(:,3) ~= 0);
crpv = crpv(:).*sign(az(idx_non0,3));
% compute the total projected path length (can be negative!)
spv = sv(jppv(:))-sv(jppv(1)) + rpv(:);
