Documente Academic
Documente Profesional
Documente Cultură
1. Realizar el algoritmo A*
% demo_03
% LIMPIAR ENTORNO
clc; clear; close all
% CONSTRUIR CELDAS
% 1. Numero de celdas por lado
m = 10;
figure();
hold on;
for i = 1:m+1
% Vertical Lines
plot([i,i],[1,m+1], "k");
plot([1,m+1],[i,i], "k");
end
handle = gcf;
% 2. Obstacles Coordinates
Obstacles = [2 3; 2 4; 2 5; 2 6; 3 3; 4 3; 5 3; 2 7];
%Obstacles = [2 3;2 4; 5 3; 4 4; 8 7; 3 5];
for i = 1:size(Obstacles,1)
fill_cell(handle, Obstacles(i,:), "k");
end
while(~isempty(L))
CIndex = search_min_distance(L, f);
Current = L(CIndex, :);
Cg = g(Current(1), Current(2));
Cf = f(Current(1), Current(2));
L(CIndex, :) = [];
if (Current(1) == Goal(1) && Current(2) == Goal(2))
break
end
for i = -1:1:1
for k = -1:1:1
if(check_boundaries_obstacles(m, Current, i, k, g))
if(g(Current(1) + i, Current(2) + k) > Cg +
round(10*norm([i,k])))
g(Current(1) + i, Current(2) + k) = Cg +
round(10*norm([i,k]));
f(Current(1) + i, Current(2) + k) = g(Current(1) + i,
Current(2) + k) + heuristic([Current(1) + i, Current(2) + k], Goal);
Parents{Current(1) + i, Current(2) + k} = Current;
L = [L; [Current(1) + i, Current(2) + k]];
end
end
end
end
end
% 5. Go through parents
node = Goal;
while(sum(abs(node - Start)) ~= 0)
node = Parents{node(1), node(2)};
fill_cell(handle, node, "y");
end
fill_cell(handle, Start, "g");