Asked by HAITHAM AL SATAI
on 15 Nov 2018

%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')

Opportunities for recent engineering grads.

Apply Today
## 2 Comments

## John D'Errico (view profile)

## Direct link to this comment

https://de.mathworks.com/matlabcentral/answers/429954-could-anyone-explain-this-code-for-me-please#comment_637528

## Walter Roberson (view profile)

## Direct link to this comment

https://de.mathworks.com/matlabcentral/answers/429954-could-anyone-explain-this-code-for-me-please#comment_637533

Sign in to comment.