PLANIFICACIÓN DE MOVIMIENTO

Alumno: García de la Cruz, Pablo Alexis 20142127E

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

% 3. Start and Goal

Start = [1 2];
Goal = [6 6];
fill_cell(handle, Start, "g");
fill_cell(handle, Goal, "r");

% 4. A* Algorithm: Diagonal movement cost is 14

% 4.1. Create a visisted matrix
g = ones(m,m)*Inf;
%h = ones(m,m)*Inf;
f = ones(m,m)*Inf;
Parents = cell(m,m);
% 4.2. Label obstacles as -2
for i = 1:size(Obstacles,1)
g(Obstacles(i,1), Obstacles(i,2)) = -2;
%h(Obstacles(i,1), Obstacles(i,2)) = -2;
f(Obstacles(i,1), Obstacles(i,2)) = -2;
end
% 4.3. We create a list
g(Start(1), Start(2)) = 0;
%h(Start(1), Start(2)) = heuristic(Start, Goal);
%f(Start(1), Start(2)) = g(Start(1), Start(2)) + h(Start(1),
Start(2));
f(Start(1), Start(2)) = g(Start(1), Start(2)) + heuristic(Start,
Goal);
L = Start;

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");

% 6. show distance matrix

for i = 1:m
for k = 1:m
add_text_astar(handle, [i,k], num2str(g(i,k)), num2str(f(i,k)));
end
end

2. Realizar el algoritmo Minimum Spanning Tree (Prim)

% demo_04
% LIMPIAR ENTORNO
clc, clear, close all;
% Inicializacion del Algoritmo
% 1. Número de vertices a conectar
n = 9;
% 2. Inicializacion del vector Q, KEY y PARENT
Q = [1; 2; 3; 4; 5; 6; 7; 8; 9];
Point_key = ones(n,1) * Inf;
Point_parent = zeros(n, 1);
random = randperm(n, 1);
% 3. Cuadro de conexiones [Vertice 1, Vertice 2, Valor_del_camino]
Co = [1 2; 1 3; 2 3; 1 5; 3 4; 1 4; 2 4; 4 6; 3 6; 4 5; 2 5];
W = [ 1; 5; 9; 6; 5; 7; 4; 3; 10; 8; 3];
AA = zeros(size(Co, 1), size(Co, 1));
% 4. Inicializacion aleatoria
Point_key(random) = 0;
A = [];
% 5. Generacion de la Matriz de Conexiones
for i = 1 : size(Co,1)
index = Co(i,:);
AA(index(1), index(2)) = W(i);
AA(index(2), index(1)) = W(i);
end
% 6. Algoritmo Prim
while (~isempty(Q))
[M, I] = min(Point_key(Q));
u = Q(I);
Q(I) = [];
if (Point_parent(u)~=0)
act = [u Point_parent(u)];
A = [A; act];
end
for i = 1 : size(Co,1)
if (Co(i, 1) == u)
v_adj = Co(i,2);
if (ismember(v_adj,Q) && AA(u,v_adj) < Point_key(v_adj))
Point_parent(v_adj) = u;
Point_key(v_adj) = AA(u,v_adj);
end
end
if (Co(i, 2) == u)
v_adj = Co(i,1);
if (ismember(v_adj,Q) && AA(u,v_adj) < Point_key(v_adj))
Point_parent(v_adj) = u;
Point_key(v_adj) = AA(u,v_adj);
end
end
end
end
disp('El camino debe ser:');
disp(A(:, end:-1:1));