45 lines
1.2 KiB
Matlab
45 lines
1.2 KiB
Matlab
function retval = planPathPRM ()
|
|
%%%%%%%%%%%%%%%%%%%
|
|
%planPathPRM ()
|
|
%ex. planPathPRM()
|
|
%
|
|
%Inputs:
|
|
%
|
|
%
|
|
%Outputs:
|
|
%
|
|
%Author : Thomas Périn, thomas.perin@ecam.fr
|
|
%Date/ 22/11/2023
|
|
%%%%%%%%%%%%%%%%%%%
|
|
|
|
load('dataFile.mat')
|
|
|
|
start = [2000; 0];
|
|
goal = [-2000; 0];
|
|
|
|
nbNodes = size(xy_valid,2);
|
|
|
|
visibilityGraph = zeros(size(xy_valid,2)+2,size(xy_valid,2)+2);
|
|
|
|
for l = 1:(size(visibilityGraph,2)-1)
|
|
for m = 1:(size(visibilityGraph,2)-1)
|
|
if l ~= m
|
|
if l == 1
|
|
visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, m-1))^2 + xy_valid(2, m-1)^2);
|
|
elseif m == 1
|
|
visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, l-1))^2 + xy_valid(2, l-1)^2);
|
|
elseif connectionMap(l-1, m-1) == 1
|
|
visibilityGraph(l , m) = sqrt((xy_valid(1, m-1)-xy_valid(1, l-1))^2 + (xy_valid(2, m-1)-xy_valid(2, l-1))^2);
|
|
end
|
|
end
|
|
end
|
|
end
|
|
for s = 2:(size(visibilityGraph,2)-1)
|
|
visibilityGraph(s , 12) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2);
|
|
visibilityGraph(12 , s) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2);
|
|
end
|
|
visibilityGraph
|
|
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph);
|
|
|
|
endfunction
|