점과 다각형 사이의 거리

Path planning 관련 코드를 작성하다보면 장애물과 점 사이의 거리를 계산해야 할 때가 있다. 직관적으로 생각하면 간단하지만, 실제로 구현하려고 하면 좀 복잡하다. 그래서 정리 차원에서 구현해보았다. 

구현을 최대한 간단하게 하기 위해서 점과 장애물 사이의 거리를 다음과 같이 정의된다. 

1. 점이 장애물 속에 들어가 있다면 -1을 출력한다. 

2. 점은 장애물 사이의 거리는 장애물의 경계면이 이루는 선분과 최소 거리로 정의된다. 

먼저 점과 직선 사이의 거리를 계산한 결과는 다음과 같다. 

점과 선분 사이의 거리

점과 다각형 사이의 거리

다각형에 마진을 추가한 영역에 속하는지 여부

코드는 다음과 같다. 

1. demo

%%

%

% 장애물의 boundary 점들이 있을 때, 임의의 점이 해당 장애물 + margin을 준 것에 속한지 여부를

% 알애내는 것이 목적이다.

%

clc; clear; close all;

%% 1. 점과 선분 사이의 거리

A = [4 1];

B = [2 9];

axis_val = [-10 15 -10 15];

figure(1);

while 1

    clf; hold on;

    plot([A(1) B(1)], [A(2) B(2)], 'bo-', 'LineWidth', 2, 'MarkerSize', 15);

    text(A(1), A(2), '  A', 'FontSize', 15);

    text(B(1), B(2), '  B', 'FontSize', 15);

    axis(axis_val); grid on;

    pos = ginput(1);

    dist = get_dist_point_line(A, B, pos);

    plot(pos(1), pos(2), 'kx', 'LineWidth', 2, 'MarkerSize', 15);

    axis(axis_val);  grid on;

    title(sprintf('Dist: %.1f', dist), 'FontSize', 15);

    drawnow;

    pause();

end

%% 2. 점과 다각형 사이의 거리 (내부에 있으면 -1)

poly = [4 1 ; 2 9 ; -5 10; -5 5; -3 5; -1 1; 4 1];

axis_val = get_axis_with_margin(poly, 3);

figure(2);

while 1

    clf; hold on;

    plot(poly(:, 1), poly(:, 2), 'bo-', 'LineWidth', 2, 'MarkerSize', 15);

    axis(axis_val); grid on;

    pos = ginput(1);

    dist = get_dist_point_poly(poly, pos);

    plot(poly(:, 1), poly(:, 2), 'bo-', 'LineWidth', 2, 'MarkerSize', 15);

    plot(pos(1), pos(2), 'kx', 'LineWidth', 2, 'MarkerSize', 15);

    axis(axis_val);  grid on;

    

    title(sprintf('Dist: %.1f', dist), 'FontSize', 15);

    drawnow;

    pause();

end

%% 3. 다각형에 마진을 준 결과

poly = [10 1 ; 2 9 ; -5 10; -5 5; -3 5; -1 1; 10 1];

axis_val = get_axis_with_margin(poly, 3);

margin = 2;

stage_struct = init_stage(axis_val(1), axis_val(2), axis_val(3), axis_val(4));

inout_grid = zeros(stage_struct.yres, stage_struct.xres);

for yi = 1:stage_struct.yres

    for xi = 1:stage_struct.xres

        curr_pos = [stage_struct.xgrid(xi) stage_struct.ygrid(yi)];

        if get_dist_point_poly(poly, curr_pos) < margin

            val = 0;

        else

            val = 1;

        end

        inout_grid(yi, xi) = val;

    end

end

figure(3);

hold on;  colormap summer;

pcolor(stage_struct.xgrid, stage_struct.ygrid, inout_grid); shading interp; lighting phong;

for i = 1:stage_struct.nr_points

    curr_pos = stage_struct.points(i, :);

    if get_dist_point_poly(poly, curr_pos) < margin

        c = 'r';

    else

        c = 'b';

    end

    plot(curr_pos(1), curr_pos(2), 'x', 'Color', c);

end

plot(poly(:, 1), poly(:, 2), 'bo-', 'LineWidth', 2, 'MarkerSize', 15);

axis(axis_val);

grid on;

2. get_dist_point_line

function dist = get_dist_point_line(pnt_a, pnt_b, pnt_x)

% 1. pnt_a == pnt_b

len_ab = norm(pnt_a - pnt_b);

if len_ab == 0

    dist = norm(pnt_a - pnt_x);

    return;

end

% 2. otherwise

prj = ((pnt_x(1)-pnt_a(1))*(pnt_b(1)-pnt_a(1)) + (pnt_x(2)-pnt_a(2))*(pnt_b(2)-pnt_a(2)))/len_ab;

if prj < 0

    dist = norm(pnt_a - pnt_x);

elseif prj > len_ab

    dist = norm(pnt_b - pnt_x);

else

    dist = abs((-(pnt_x(1)-pnt_a(1))*(pnt_b(2)-pnt_a(2)) + (pnt_x(2)-pnt_a(2))*(pnt_b(1)-pnt_a(1)))/len_ab);

end

3. get_dist_point_poly

function dist = get_dist_point_poly(poly, pnt_x)

dist = inf;

for i = 1:size(poly, 1)-1

    A = poly(i, :);

    B = poly(i+1, :);

    curr_dist = get_dist_point_line(A, B, pnt_x);

    if curr_dist < dist

        dist = curr_dist;

    end

end

% 만약 polygon 안에 있다면 

if inpolygon(pnt_x(1), pnt_x(2), poly(:,1), poly(:,2))

    dist = -1;

end

4. get_axis_with_margin

function axis_val = get_axis_with_margin(boundary, margin)

xmin = min(boundary(:, 1));

ymin = min(boundary(:, 2));

xmax = max(boundary(:, 1));

ymax = max(boundary(:, 2));

axis_val = [xmin-margin xmax+margin ymin-margin ymax+margin];

5. init_stage

function stage_struct = init_stage(xmin, xmax, ymin, ymax, rgb_color)

if nargin == 4

    rgb_color = rand(1, 3);

end

stage_struct.w = xmax - xmin;

stage_struct.h = ymax - ymin;

stage_struct.boundary  ...

    = makeBoundary([xmin ymin ; xmax ymin ; xmax ymax ; xmin ymax]);

stage_struct.rgb_color = rgb_color;

% 순서대로 xmin xmax ymin ymax

stage_struct.axis = [xmin xmax ymin ymax];

stage_struct.xmin = stage_struct.axis(1);

stage_struct.xmax = stage_struct.axis(2);

stage_struct.ymin = stage_struct.axis(3);

stage_struct.ymax = stage_struct.axis(4);

stage_struct.xlen = stage_struct.xmax - stage_struct.xmin;

stage_struct.ylen = stage_struct.ymax - stage_struct.ymin;

% stage에 grid를 설정한다.

stage_struct.xres = 30;

stage_struct.yres = 30;

stage_struct.xlen = stage_struct.xmax - stage_struct.xmin;

stage_struct.ylen = stage_struct.ymax - stage_struct.ymin;

stage_struct.xgrid = linspace(stage_struct.xmin, stage_struct.xmax, stage_struct.xres);

stage_struct.ygrid = linspace(stage_struct.ymin, stage_struct.ymax, stage_struct.yres);

[mesh_X, mesh_Y] = meshgrid(stage_struct.xgrid, stage_struct.ygrid);

stage_struct.xmesh = mesh_X;

stage_struct.ymesh = mesh_Y;

stage_struct.points = [mesh_X(:) mesh_Y(:)];

stage_struct.nr_points = length(stage_struct.points);

stage_struct.pred_pos ...

    = [reshape(stage_struct.xmesh, [], 1), reshape((stage_struct.ymesh), [], 1)];

function boundary = makeBoundary(corners)

boundary = [corners ; corners(1, :)];