% PRM 与えた点からグラフへのアクセス

% 下記の条件を満たすグラフ上の点を求める
%   与えた点 p とグラフ上の点との距離がなるべく短い
%   与えた点 p とグラフ上の点を結ぶ線分が自由

function [ num, dist] = PRM_access_from_point (p, points, npoints)
    num = 0;
    dist = 1e+10;
    for k=1:npoints
        d = norm(p - points(k,:));
        if (d < dist)
            if (PRM_free_edge_check(p, points(k,:)) == 1)
                num = k;
                dist = d;
            end
        end
    end
end
