test_path_generator2.m

pathgenerator source, filtering for matlab

%%%%%%%%%%%%%%%%%%%%% A example where V random paths are generated with 2
%%%%%%%%%%%%%%%%%%%%% exclusion areas %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



clear, clc , close all hidden;


V             = 40;                  % Number of paths to generate
beta          = 0.2;                 % K = round(Ks*(1 + beta))
L             = 1;
Ny            = 50;
Nx            = 50;


Y_min         = 0.0 ;               % m
Y_max         = 1.0;                % m
X_min         = 0.0 ;               % m
X_max         = 1.0;                % m

diff_Y        = abs(Y_max - Y_min);
diff_X        = abs(X_max - X_min);
pente_Y       = diff_Y/(Ny - 1);
pente_X       = diff_X/(Nx - 1);
vect_X        = (X_min : pente_X : X_max);
vect_Y        = (Y_min : pente_Y : Y_max);
[x , y]       = ndgrid(vect_X , vect_Y );
XY            = [x(:)' ; y(:)'];

S             = Nx*Ny;

I             = indice_ij(Nx , Ny);
delta         = circulant([1 1 0 0 0 0 0 1 ]);


figure(1)
axis xy
hold on
plot(XY(1 , :) , XY(2 , :) , '+')

hh            = title('Select the first exlusion zone');
set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')

[xx , yy]     = getline(gcf , 'closed');
node          = [xx' ; yy'];

hold on
plot(xx , yy , 'r', 'linewidth' , 2)
[in , bnd]    = inpoly(XY , node);
exclu         = find(in)';
Xexclu        = XY(1,exclu);
Yexclu        = XY(2,exclu);

plot(Xexclu , Yexclu , 'ro', 'linewidth' , 2 )

I             = exclusion(I , exclu);
delta         = circulant([1 1 0 0 0 0 0 1 ]);


hh            = title('Select the second exlusion zone');
set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')
[xx , yy]     = getline(gcf , 'closed');
node          = [xx' ; yy'];

hold on
plot(xx , yy , 'r', 'linewidth' , 2)
[in , bnd]    = inpoly(XY , node);
exclu         = find(in)';
Xexclu        = XY(1,exclu);
Yexclu        = XY(2,exclu);
plot(Xexclu , Yexclu , 'ro', 'linewidth' , 2 )

I             = exclusion(I , exclu);


hh            = title('Select start zone');
set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')


[xx , yy]     = getline(gcf , 'closed');
node          = [xx' ; yy'];
plot(xx , yy , 'k', 'linewidth' , 2)
[in , bnd]    = inpoly(XY , node);
plot(XY(1 , in) , XY(2 , in) , 'y*', 'linewidth' , 2 )
start         = find(in)';


hh            = title('Select finish zone');
set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')


[xx , yy]     = getline(gcf , 'closed');
node          = [xx' ; yy'];
plot(xx , yy , 'k', 'linewidth' , 2)
[in , bnd]    = inpoly(XY , node);
plot(XY(1 , in) , XY(2 , in) , 'm^', 'linewidth' , 2 )
finish        = find(in)';
drawnow


%%%%%%%%%%% Shortest path %%%%%%%%%%

% C                                 = mat_dist(XY , I);
% index_Y_spath                     = mdp_contraintes3(start, finish, I , -C , delta);

C                                 = mat_dist(XY , I , 0);
[i , j , s]                       = find(I);
A                                 = sparse(s , j , C(s) , S , S);
index_Y_spath                     = dijkstra(A , start , finish);


Y_spath                           = XY(: , index_Y_spath);
spath_cost                        = sum(sqrt(sum(diff(Y_spath , 1 , 2).^2)));


plot(Y_spath(1 , :) , Y_spath(2 , :) , 'y' , XY(1 , start) ,  XY(2 , start) , '^' , XY(1 , finish) , XY(2 , finish ) , 'sg'  , 'linewidth' , 2 )
drawnow
K                                 = max(1 , round(length(Y_spath)*(1 + beta)));


%%%%%%%%%%% Generate random Paths  %%%%%%%%%%

C                                 = mat_uni(I);  % Uniform pdf matrix
P                                 = path_generator2(start, finish , I , C , K , V, delta);

H                                 = reshape(XY(: , P) , [2 , K , L , V]);
plot( squeeze(H(1 , : , :)) , squeeze(H(2 , : , :)) , XY(1 , start) ,  XY(2 , start) , '^' , XY(1 , finish) ,  XY(2 , finish ) , 'sg'  , 'linewidth' , 2 )
hold off
title(sprintf('Generated paths, V = %d, K = %d' , V , K), 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold');