A星算法matlab源码及详细注释

functionastardemo%ASTARDEMODemonstrationofASTARalgorithm%%CopyrightBobL.Sturm,Ph.D.,AssistantProfessor%DepartmentofArchitecture,DesignandMediaTechnology%formerlyMedialogy%AalborgUniversityiBallerup%formerlyAalborgUniversityCopenhagen%$Revision:0.1$$Date:2011Jan.1518h24:24$n=20;%fieldsizenxntiles20*20的界面wallpercent=0.45;%thispercentoffieldiswalls45%的界面作为阻碍物（墙）%createthenxnFIELDwithwallpercentwallscontainingmovementcosts,%astartingpositionSTARTPOSIND,agoalpositionGOALPOSIND,thecosts%AstarwillcomputemovementcostforeachtileCOSTCHART,%andamatrixinwhichtostorethepointersFIELDPOINTERS[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent);%初始化界面%initializetheOPENandCLOSEDsetsandtheircostssetOpen=[startposind];setOpenCosts=[0];setOpenHeuristics=[Inf];setClosed=[];setClosedCosts=[];movementdirections={'R','L','D','U'};%keeptrackofthenumberofiterationstoexitgracefullyifnosolutioncounterIterations=1;%createfiguresowecanwitnessthemagicaxishandle=createFigure(field,costchart,startposind,goalposind);%aslongaswehavenotfoundthegoalorrunoutofspacestoexplorewhile~max(ismember(setOpen,goalposind))&&~isempty(setOpen)%ismember(A,B)返回与A同大小的矩阵，其中元素1表示A中相应位置的元素在B中也出现，0则是没有出现%fortheelementinOPENwiththesmallestcost[temp,ii]=min(setOpenCosts+setOpenHeuristics);%从OPEN表中选择花费最低的点temp,ii是其下标(也就是标号索引)%findcostsandheuristicofmovingtoneighborspacestogoal%inorder'R','L','D','U'[costs,heuristics,posinds]=findFValue(setOpen(ii),setOpenCosts(ii),...field,goalposind,'euclidean');%扩展temp的四个方向点，获得其坐标posinds，各个方向点的实际代价costs，启发代价heuristics%putnodeinCLOSEDandrecorditscostsetClosed=[setClosed;setOpen(ii)];%将temp插入CLOSE表中setClosedCosts=[setClosedCosts;setOpenCosts(ii)];%将temp的花费计入ClosedCosts%updateOPENandtheirassociatedcosts更新OPEN表分为三种情况if(ii1&&iilength(setOpen))%temp在OPEN表的中间，删除tempsetOpen=[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(ii==1)setOpen=setOpen(2:end);%temp是OPEN表的第一个元素，删除tempsetOpenCosts=setOpenCosts(2:end);setOpenHeuristics=setOpenHeuristics(2:end);else%temp是OPEN表的最后一个元素，删除tempsetOpen=setOpen(1:end-1);setOpenCosts=setOpenCosts(1:end-1);setOpenHeuristics=setOpenHeuristics(1:end-1);end%foreachoftheseneighborspaces,assigncostsandpointers;%andifsomeareintheCLOSEDsetandtheircostsaresmaller,%updatetheircostsandpointersforjj=1:length(posinds)%对于扩展的四个方向的坐标%ifcostinfinite,thenit'sawall,soignoreif~isinf(costs(jj))%如果此点的实际代价不为Inf,也就是没有遇到墙%ifnodeisnotinOPENorCLOSEDtheninsertintocostchartand%movementpointers,andputnodeinOPENif~max([setClosed;setOpen]==posinds(jj))%如果此点不在OPEN表和CLOSE表中fieldpointers(posinds(jj))=movementdirections(jj);%将此点的方向存在对应的fieldpointers中costchart(posinds(jj))=costs(jj);%将实际代价值存入对应的costchart中setOpen=[setOpen;posinds(jj)];%将此点加入OPEN表中setOpenCosts=[setOpenCosts;costs(jj)];%更新OPEN表实际代价setOpenHeuristics=[setOpenHeuristics;heuristics(jj)];%更新OPEN表启发代价%elsenodehasalreadybeenseen,sochecktoseeifwehave%foundabetterroutetoit.elseifmax(setOpen==posinds(jj))%如果此点在OPEN表中I=find(setOpen==posinds(jj));%找到此点在OPEN表中的位置%updateifwehaveabetterrouteifsetOpenCosts(I)costs(jj)%如果在OPEN表中的此点实际代价比现在所得的大costchart(setOpen(I))=costs(jj);%将当前的代价存入costchart中，注意此点在costchart中的坐标与其自身坐标是一致的（setOpen(I)其实就是posinds(jj)），下同fieldpointerssetOpenCosts(I)=costs(jj);%更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics(I)=heuristics(jj);%更新OPEN表中的此点启发代价(窃以为这个是没有变的)fieldpointers(setOpen(I))=movementdirections(jj);%更新此点的方向end%elsenodehasalreadybeenCLOSED,sochecktoseeifwehave%foundabetterroutetoit.else%如果此点在CLOSE表中，说明已经扩展过此点%findrelevantnodeinCLOSEDI=find(setClosed==posinds(jj));%updateifwehaveabetterrouteifsetClosedCosts(I)costs(jj)%如果在CLOSE表中的此点实际代价比现在所得的大（有一个问题，经过此点扩展的点还需要更新当前代价呢!!!）costchart(setClosed(I))=costs(jj);%将当前的代价存入costchart中setClosedCosts(I)=costs(jj);%更新CLOSE表中的此点代价fieldpointers(setClosed(I))=movementdirections(jj);%更新此点的方向endendendendifisempty(setOpen)break;end%当OPEN表为空，代表可以经过的所有点已经查询完毕set(axishandle,'CData',[costchartcostchart(:,end);costchart(end,:)costchart(end,end)]);%hacktomakeimagelookrightset(gca,'CLim',[01.1*max(costchart(find(costchartInf)))]);%CLim将CData中的值与colormap对应起来:[cmincmax]Coloraxislimits（不过不太明白为什么要*1.1）drawnow;%cministhevalueofthedatamappedtothefirstcolorinthecolormap.cmaxisthevalueofthedatamappedtothelastcolorinthecolormapendifmax(ismember(setOpen,goalposind))%当找到目标点时disp('Solutionfound!');%disp：Displayarray，disp(X)直接将矩阵显示出来，不显示其名字，如果X为string，就直接输出文字X%nowfindthewaybackusingFIELDPOINTERS,startingfromgoalpositionp=findWayBack(goalposind,fieldpointers);%plotfinalpathplot(p(:,2)+0.5,p(:,1)+0.5,'Color',0.2*ones(3,1),'LineWidth',4);drawnow;elseifisempty(setOpen)disp('NoSolution!');end%endofthemainfunction%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionp=findWayBack(goalposind,fieldpointers)%Thisfunctionwillfollowthepointersfromthegoalpositiontothe%startingpositionn=length(fieldpointers);%lengthofthefieldposind=goalposind;%convertlinearindexinto[rowcolumn][py,px]=i