Calculate distance point to line segment with special cases

Question

I need to implement the following logic: Given a set of 2d sample points (given as x-y-coordinate pairs) and a set of line segments (also x-y-coordinate pairs). Edit 1: How to calculate (vectorized) the distance of the points pi to the lines Li?

The points are approximately near the lines and I want to get the distance from each of the sample points to the nearest line segment. The may be points, which are a bit "off" (see p6 in the first picture) and these could be found by the following algorithm:

1. Case: The sample points projection to the line segment is "left outside". In this case I need the euclidean distance from p1 to x1.
2. Case: The sample points projection to the line segment is "inside" the line segment. In this case I need the distance from p2 to the line from x1 to x2.
3. Case: The sample points projection to the line segment is "right outside". In this case I need the euclidean distance from p3 to x2.

There is a vectorized solution (thanks to User Andy) which uses the projection "globally", always assuming case 2 for each point-linesegment-pair. However, this returns the distance [1 1 1] for p1 ... p3 , where the desired distance would be [1.4142 1 1.4142]. Can this code be modified to these needs?

ptsx = [1 3 5];
ptsy= [1 1 1];

linesx = [2 4];
linesy = [0 0];

points = [ptsx;ptsy];
lines = [linesx;linesy];

% start of lines
l = [linesx(1:end-1);linesy(1:end-1)];
% vector perpendicular on line
v = [diff(linesy);-diff(linesx)];
% make unit vector
v = v ./ hypot (v(1,:),v(2,:));
v = repmat (v, 1, 1, size (points, 2));
% vector from points (in third dimension) to start of lines (second dimension)
r = bsxfun (@minus, permute (points, [1 3 2]), l);
d = abs (dot (v, r));
dist = squeeze (min (d, [], 2))

Mathematically speaking, the cases can be separated by looking at the length of the projection of vec(pi-x1) onto vec(x2-x1). If this length factor is < 0, then the point is "left" of the line segment, if it is between 0 and 1, perpendicular projection is possible, if it is > 1, then the point is "right" of the line segment.

Edit 1: I will add a pseudocode to explain how this could be solved with a double for-loop, but since I have around 6000 samples and 10000 lines, the loop solution is not an option for me.

for each sample point pi
  for each linesegment Li
    a = vector from start of Li to end of Li
    b = vector from pi to start of Li
    relLength = dot(a,b)/norm(a)^2
    if relLength < 0: distance = euclidean distance from start of Li to pi
    if relLength > 1: distance = euclidean distance from end of Li to pi
    else: distance = perpendicular distance from pi to Li
  endfor 
endfor

Edit 2 / 2017-09-07: I managed to vectorize the first part of this algorithm. relLength now contains the relative length of the projection of each pi-startOfLi onto each line segment.

ptsx = [0.5 2 3 5.5 8 11];
ptsy= [1  2 -1.5 0.5 4 5];

linesx = [0 2 4 6 10 10 0 0];
linesy = [0 0 0 0  0  4 4 0];

points = [ptsx;ptsy];
lines = [linesx;linesy];

% Start of all lines
L1 = [linesx(1:end-1); linesy(1:end-1)];
% Vector of each line segment
a = [diff(linesx); diff(linesy)];
b = bsxfun(@minus, permute(points, [1 3 2]), L1);
aRep = repmat(a, 1, 1, length(b(1,1,:)));
relLength = dot(aRep,b)./norm(a, 'cols').^2

Answer 1

In GNU Octave:

points = [1 4.3 3.7 2.9;0.8 0.8 2.1 -0.5];
lines = [0 2 4 3.6;0 -1 1 1.75];

% plot them
hold off
plot (points(1,:), points(2,:), 'or')
hold on
plot (lines(1,:), lines(2,:), '-xb')
text (points(1,:), points(2,:),...
      arrayfun (@(x) sprintf('  p%i',x),...
                1:columns(points),'UniformOutput', false))
axis ('equal')
grid on
zoom (0.9);

% some intermediate vars
s_lines = lines (:,1:end-1);    % start of lines
d_lines = diff(lines, 1, 2);    % vectors between line points
l_lines = hypot (d_lines(1,:),
                 d_lines(2,:)); % length of lines

now do the "real" work:

% vectors perpendicular on lines
v = [0 1;-1 0] * d_lines;
vn = v ./ norm (v, 2, 'cols');   %make unit vector

% extend to number of points
vn = repmat (vn, 1, 1, columns (points));
points_3 = permute (points, [1 3 2]);

% vectors from points (in third dimension) to start of lines (second dimension)
d =  dot (vn, points_3 - s_lines);

% check if the projection is on line
tmp = dot (repmat (d_lines, 1, 1, columns (points)),...
           points_3 - s_lines)./l_lines.^2;
point_hits_line = tmp > 0 & tmp < 1;

% set othogonal distance to Inf if there is no hit
d(~ point_hits_line) = Inf;

dist = squeeze (min (abs(d), [], 2));

% calculate the euclidean distance from points to line start/stop
% if the projection doesn't hit the line
nh = isinf (dist);
tmp = points_3(:,:,nh) - lines;
tmp = hypot(tmp(1,:,:),tmp(2,:,:));
tmp = min (tmp, [], 2);
% store the result back
dist (nh) = tmp

plot the results as yellow circles around the points

% use for loops because this hasn't to be fast
t = linspace (0, 2*pi, 40);
for k=1:numel(dist)
  plot (points (1, k) + cos (t) * dist(k),
        points (2, k) + sin (t) * dist(k),
        '-y')
end

Answer 2

Use octaves Geometry package

Octaves geometry package contains all necessary tools to solve the requested problem. There are two functions implementing the solution for your question:

The function distancePointPolyline and distancePointPolygon should both be able to calculate the requested distances. Polygones are closed polylines.

Example

The following script demonstrates the use of the functions. See figure for graphical result.

% Load octave geometry package (package is also available for matlab)
pkg load geometry
% Define points
points = [1,4.3,3.7,2.9; 0.8, 0.8, 2.1, -0.5]';
% Define polyline
lines  = [0, 2, 4, 3.6;   0, -1, 1, 1.75]';

% Calculate distance
d = distancePointPolyline (points,lines);

% Produce figure
figure('name','Distance from points to polyline');
hold all
drawPoint(points);
drawPolyline(lines);
drawCircle(points, d);
axis equal

Result

matlaboctave