clc;
clear;
close all;
n=20;
wallpercent=0.1;
[ field,startposind,goalposind,costchart,fieldpointers ] = initializefield( n,wallpercent );
axishandle = creatureFigure( field,startposind,goalposind );
setopen=[startposind];setopencosts=[0];setopenheuristics=[inf];
setclose=[];setclosecosts=[];
movementdirection=['R','L','D','U'];
while ~max(ismember(setopen,goalposind))&&~isempty(setopen)
[temp,ii]=min(setopencosts+setopenheuristics);
[costs,heuristics,posinds]=findfvalue(setopen(ii),setopencosts(ii),field,goalposind);
setclose=[setclose;setopen(ii)];setclosecosts=[setclosecosts;setopencosts(ii)];
if ii>1&&ii<length(setopen)
setopen=[setopen(1:ii-1);setopen(ii+1:end)];
setopencosts=[setopencosts(1:ii-1);setopencosts(ii+1:end)];
setopenheuristics=[setopenheuristics(1:ii-1);setopenheuristics(ii+1:end)];
elseif 1==ii
setopen=[setopen(ii+1:end)];
setopencosts=[setopencosts(ii+1:end)];
setopenheuristics=[setopenheuristics(ii+1:end)];
else
setopen=[setopen(ii+1:end)];
setopencosts=[setopencosts(ii+1:end)];
setopenheuristics=[setopenheuristics(ii+1:end)];
end
for jj=1:length(posinds)
if ~isinf(costs(jj))
if ~max([setopen;setclose]==posinds(jj))
fieldpointers{posinds(jj)}=movementdirection(jj);
setopen = [setopen; posinds(jj)];
setopencosts = [setopencosts; costs(jj)];
setopenheuristics = [setopenheuristics; heuristics(jj)];
elseif max(setopen==posinds(jj))
i=find(setopen==posinds(jj));
if setopencosts(i)>costs(jj)
setopencosts(i)=costs(jj);
setopenheuristics(i)=heuristics(jj);
fieldpointers{setopen(i)}=movementdirection(jj);
end
else i=find(setclose==posinds(jj));
if setclosecosts(i)>costs(jj)
setclosecosts(i)=costs(jj);
fieldpointers{setclose(i)}=movementdirection(jj);
end
end
end
end
if isempty(setopen)
break;
end
end
if max(ismember(setopen,goalposind))
p=findwayback(goalposind,fieldpointers);
p=[p;inf*ones(2)];
for kk=1:length(p)
if p(kk+2,1)==inf||p(kk+1,1)==inf
break;
elseif abs(p(kk+2,1)-p(kk,1))==1&&abs(p(kk+2,2)-p(kk,2))==1
if p(kk+1,1)-p(kk,1)==1&&p(kk+2,2)-p(kk+1,2)==1
if field(p(kk,2)+1,p(kk,1))==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
elseif p(kk+1,2)-p(kk,2)==1&&p(kk+2,1)-p(kk+1,1)==1
if field(p(kk,2),p(kk,1)+1)==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
elseif p(kk+1,1)-p(kk,1)==-1&&p(kk+2,2)-p(kk+1,2)==1
if field(p(kk,2)+1,p(kk,1))==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
elseif p(kk+1,2)-p(kk,2)==1&&p(kk+2,1)-p(kk+1,1)==-1
if field(p(kk,2),p(kk,1)-1)==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
elseif p(kk+1,1)-p(kk,1)==-1&&p(kk+2,2)-p(kk+1,2)==-1
if field(p(kk,2)-1,p(kk,1))==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
elseif p(kk+1,2)-p(kk,2)==-1&&p(kk+2,1)-p(kk+1,1)==-1
if field(p(kk,2),p(kk,1)-1)==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
elseif p(kk+1,1)-p(kk,1)==1&&p(kk+2,2)-p(kk+1,2)==-1
if field(p(kk,2)-1,p(kk,1))==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
else p(kk+1,2)-p(kk,2)==-1&&p(kk+2,1)-p(kk+1,1)==1
if field(p(kk,2),p(kk,1)+1)==inf
p;
else p=[p(1:kk,:);p(kk+2:end,:)];
end
end
else
p;
end
end
p=p(1:end-2,:);
plot(p(:,1)+0.5,p(:,2)+0.5,'color',[0.95 0.95 0],'LineWidth',5);drawnow;
elseif isempty(setopen)
disp('no solution');
end
C81