% PRM 最短経路を見つける

% 経路探索に必要な変数をファイル 'PRM.mat' から読み込む
% ファイル PRM_making_graph.m において
% save('PRM.mat', 'g', 'x', 'y', 'points', 'npoints')
% で保存された変数

% 始点 ps から終点 pe に至る最短経路を見つける
% 結果は fileid を含むファイル名のファイルに保存

%ps = [0.5,3.5]; pe = [9.5,0.5]; fileid = '1';
ps = [0.8,1.2]; pe = [8.0,3.8]; fileid = '2';

load('PRM.mat');
clf;

if PRM_free_point_check(ps) == 0
    fprintf("始点が自由でない．\n");
    exit;
end
if PRM_free_point_check(pe) == 0
    fprintf("終点が自由でない．\n");
    exit;
end

ns = PRM_access_from_point(ps, points, npoints);
ne = PRM_access_from_point(pe, points, npoints);
path = shortestpath(g, ns, ne);
path

plot(g, 'XData',x,'YData',y);
xlim([-0.5, 10.5]);
ylim([-0.5,  5.5]);
%axis equal;
pbaspect([11 6 1]);
hold on;

pathx = [ ps(1); points(path,1); pe(1) ];
pathy = [ ps(2); points(path,2); pe(2) ];
plot(pathx,pathy,'r-', 'LineWidth',3);
hold off;
filename = strcat('PRM_graph_shortest_path_', fileid, '.png');
saveas(gcf, filename, 'png');
fprintf("始点から終点を指定し，最短経路を求める\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(pathx,pathy,'r-', 'LineWidth',3);
xlim([-0.5, 10.5]);
ylim([-0.5,  5.5]);
%axis equal;
pbaspect([11 6 1]);
filename = strcat('PRM_shortest_path_', fileid, '.png');
saveas(gcf, filename, 'png');
fprintf("最短経路\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;
