A*算法 matlab版

function astardemo
%ASTARDEMO Demonstration of ASTAR algorithm

n = 20;   % field size n x n tiles
wallpercent = 0.45;  % this percent of field is walls

% create the n x n FIELD with wallpercent walls containing movement costs 
% a starting position STARTPOSIND a goal position GOALPOSIND the costs 
% A star will compute movement cost for each tile COSTCHART 
% and a matrix in which to store the pointers FIELDPOINTERS
[field startposind goalposind costchart fieldpointers] = ...
  initializeField（nwallpercent）;

% initialize the OPEN and CLOSED sets and their costs
setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
movementdirections = {‘R‘‘L‘‘D‘‘U‘};

% keep track of the number of iterations to exit gracefully if no solution
counterIterations = 1;

% create figure so we can witness the magic
axishandle = createFigure（fieldcostchartstartposindgoalposind）;

% as long as we have not found the goal or run out of spaces to explore
while ~max（ismember（setOpengoalposind）） && ~isempty（setOpen）
  % for the element in OPEN with the smallest cost
  [temp ii] = min（setOpenCosts + setOpenHeuristics）;
  % find costs and heuristic of moving to neighbor spaces to goal
  % in order ‘R‘‘L‘‘D‘‘U‘
  [costsheuristicsposinds] = findFValue（setOpen（ii）setOpenCosts（ii） ...
    fieldgoalposind‘euclidean‘）;
  % put node in CLOSED and record its cost
  setClosed = [setClosed; setOpen（ii）];
  setClosedCosts = [setClosedCosts; setOpenCosts（ii）];
  % update OPEN and their associated costs
  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 （ii == 1）
    setOpen = setOpen（2:end）;
    setOpenCosts = setOpenCosts（2:end）;
    setOpenHeuristics = setOpenHeuristics（2:end）;
  else
    setOpen = setOpen（1:end-1）;
    setOpenCosts = setOpenCosts（1:end-1）;
    setOpenHeuristics = setOpenHeuristics（1:end-1）;
  end
  % for each of these neighbor spaces assign costs and pointers; 
  % and if some are in the CLOSED set and their costs are smaller 
  % update their costs and pointers
  for jj=1:length（posinds）
    % if cost infinite then it‘s a wall so ignore
    if ~isinf（costs（jj））
      % if node is not in OPEN or CLOSED then insert into costchart and 
      % movement pointers and put node in OPEN
      if ~max（[setClosed; setOpen] == posinds（jj））
        fieldpointers（posinds（jj）） = movementdirections（jj）;
        costchart（posinds（jj）） = costs（jj）;
        setOpen = [setOpen; posinds（jj）];
        setOpenCosts = [setOpenCosts; costs（jj）];
        setOpenHeuristics = [setOpenHeuristics; heuristics（jj）];
      % else node has already been seen so check to see if we have
      % found a better route to it.
      elseif max（setOpen == posinds（jj））
        I = find（setOpen == posinds（jj））;
        % update if