Could anyone explain this code for me please?

1 view (last 30 days)
%A star algorithm
tic
%Initialize
%Node from which the good neighbour is reached
came_fromx=zeros(size(E)); %???!!
came_fromy=zeros(size(E));
%Nodes already evaluated
closed=[];
%Nodes to be evaluated
open=[y0,x0];
%Cost of moving from start point to the node
G=elev*ones(size(E));
G(y0,x0)=0;
%Cost of moving from the node to the arrival point
H=elev*ones(size(E));
H(y0,x0)=sqrt((xend-x0)^2+(yend-y0)^2);
% H(y0,x0)=octile_distance(x0,y0,xend,yend);
%Initial total cost
F=G+H;
%Cost weight
kg=0.5;
kh=0.8;
ke=0.5;
%Initialize
exit=0;
k=1;
%While open is not empty
while isempty(open)==0 && exit==0
%Find node with minimum f in open
%Initialize
f_open=zeros(size(open,1),1);
%Evaluate f for the nodes in open
for i=1:size(open,1)
f_open(i,:)=F(open(i,1),open(i,2));
end
%Find the index location in open for the node with minimum f
[~,i_f_open_min]=min(f_open);
%Location of node with minimum f in open
ycurrent=open(i_f_open_min,1);
xcurrent=open(i_f_open_min,2);
% %Path search video
% x=1:d_grid:x_size;
% y=1:d_grid:y_size;
% ax=figure(99);
% surf(x,y,E)
% hold on
% plot3(xcurrent,ycurrent,elev,'g*')
% plot3(x0,y0,elev,'go')
% plot3(xend,yend,elev,'ro')
% frames(k) = getframe(ax);
% axis tight
% view(0, 90);
% k=k+1;
%Check if the arrival node is reached
if xcurrent==xend && ycurrent==yend
%Arrival node reached, exit and generate path
exit=1;
else
%Add the node to closed
closed(size(closed,1)+1,:)=[ycurrent, xcurrent];
%Remove the node from open
i_open_keep=horzcat(1:i_f_open_min-1,i_f_open_min+1:size(open,1));
open=open(i_open_keep,:);
%Check neighbour nodes
for i=-1:1
for j=-1:1
%If the neighbour node is within grid
if xcurrent+i>0 && ycurrent+j>0 && xcurrent+i<=x_size && ycurrent+j<=y_size
%If the neighbour node does not belong to open and to closed
if sum(xcurrent+i==open(ycurrent+j==open(:,1),2))==0 && sum(xcurrent+i==closed(ycurrent+j==closed(:,1),2))==0
%Add the neighbour node to open
open(size(open,1)+1,:)=[ycurrent+j, xcurrent+i];
%Evaluate the distance from start to the current node + from the current node to the neighbour node
g_try=G(ycurrent,xcurrent)+sqrt(i^2+j^2);
%If this distance is smaller than the neighbour distance
if g_try<G(ycurrent+j,xcurrent+i)
%We are on the good path, save information
%Record from which node the good neighbour is reached
came_fromy(ycurrent+j,xcurrent+i)=ycurrent;
came_fromx(ycurrent+j,xcurrent+i)=xcurrent;
%Evaluate the cost function
G(ycurrent+j,xcurrent+i)=g_try;
H(ycurrent+j,xcurrent+i)=sqrt((xend-(xcurrent+i))^2+(yend-(ycurrent+j))^2);
% H(ycurrent+j,xcurrent+i)=octile_distance(xcurrent+i,ycurrent+j,xend,yend);
F(ycurrent+j,xcurrent+i)=kg*G(ycurrent+j,xcurrent+i)+kh*H(ycurrent+j,xcurrent+i)+ke*E(ycurrent+j,xcurrent+i);
end
end
end
end
end
end
end
%Reconstruct path backwards knowing from which node the good neighbour is reached
%First element is the arrival point
path_backwards=[ycurrent,xcurrent];
%Initialize
i=2;
%While the start point is not reached
while xcurrent~=x0 || ycurrent~=y0
path_backwards(i,:)=[came_fromy(ycurrent,xcurrent) came_fromx(ycurrent,xcurrent)];
ycurrent=path_backwards(i,1);
xcurrent=path_backwards(i,2);
i=i+1;
end
clear i
t_path=toc
%Number of waypoints
n_points=size(path_backwards,1);
%Initialize
path=zeros(n_points,2);
path_distance=zeros(n_points,1);
theta=zeros(n_points-1,1);
for i=1:n_points
%Reverse path sequence
path(i,:)=path_backwards(n_points+1-i,:);
if i>=2
path_distance(i)=path_distance(i-1)+sqrt((path(i,2)-path(i-1,2))^2+(path(i,1)-path(i-1,1))^2);
theta(i-1)=atan2(path(i,1)-path(i-1,1),path(i,2)-path(i-1,2));
end
end
%Angle variations
d_theta=diff(theta);
for i=1:size(d_theta,1)
if d_theta(i)<-pi
d_theta(i)=d_theta(i)+2*pi;
elseif d_theta(i)>pi
d_theta(i)=d_theta(i)-2*pi;
end
end
%Cumulative angle variations
d_theta_sum=sum(abs(d_theta));
%Segment length
distance_segment=diff(path_distance);
%Distance of each segment with respect to all path [%]
fraction_segment_perc=distance_segment/path_distance(end)*100;
%Ratio between path lenght and direct line distance start-end
distance_ratio=path_distance(end)/line_distance;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot
%Grid side vectors
x=1:d_grid:x_size;
y=1:d_grid:y_size;
%Path elevation
h_vect=h*ones(size(path,1),1);
%Open elevation
h_vect_open=h*ones(size(open,1),1);
%Closed elevation
h_vect_closed=h*ones(size(closed,1),1);
figure(1)
surf(x,y,E)
hold on
plot3(x0,y0,h,'go')
plot3(xend,yend,h,'ro')
plot3(path(:,2),path(:,1),h_vect,'c+')
plot3(path(:,2),path(:,1),h_vect,'g')
axis tight
view(0, 90);
colorbar
figure(2)
contourf(x,y,E,20)
hold on
plot3(x0,y0,h,'go')
plot3(xend,yend,h,'ro')
plot3(path(:,2),path(:,1),h_vect,'c+')
plot3(path(:,2),path(:,1),h_vect,'g')
axis tight
colorbar
figure(3)
surf(x,y,E)
hold on
plot3(x0,y0,h,'go')
plot3(xend,yend,h,'ro')
plot3(open(:,2),open(:,1),h_vect_open,'mx')
plot3(closed(:,2),closed(:,1),h_vect_closed,'cx')
axis tight
view(0, 90);
colorbar
figure(4)
plot(path_distance)
hold on
grid on
plot(line_distance*ones(size(path_distance,1),1))
legend('Path distance','Line distance')
  2 Comments
John D'Errico
John D'Errico on 15 Nov 2018
Contact the author. Wanting an explanation of a lengthy code that does something we are not even told is virtually impossible. That would mean we would need to write a complete MATLAB manual for you. And the nice thing is? The complete set of documentiion is already written. READ THE MANUAL. Learn MATLAB.
If you have a specfific question, about a specific line? Ask it.

Sign in to comment.

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!