发布时间 : 星期四 文章A星算法matlab源码及详细注释更新完毕开始阅读65143694d5d8d15abe23482fb4daa58da0111c7a
function [cost,heuristic,posinds] = findFValue(posind,costsofar,field, ... goalind,heuristicmethod)
% This function finds the movement COST for each tile surrounding POSIND in % FIELD, returns their position indices POSINDS. They are ordered: right, % left, down, up.
n = length(field); % length of the field % convert linear index into [row column]
[currentpos(1) currentpos(2)] = ind2sub([n n],posind); %获得当前点的行列坐标，注意currentpos(1)是列坐标，currentpos(2)是行坐标
[goalpos(1) goalpos(2)] = ind2sub([n n],goalind); %获得目标点的行列坐标 % places to store movement cost value and position
cost = Inf*ones(4,1); heuristic = Inf*ones(4,1); pos = ones(4,2);
% if we can look left, we move from the right 向左查询，那么就是从右边来 newx = currentpos(2) - 1; n
ewy = currentpos(1); if newx > 0 %如果没有到边界
pos(1,:) = [newy newx]; %获得新的坐标 switch lower(heuristicmethod)
case 'euclidean' %欧几里得距离(不像啊，亲)
heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); %heuristic(1)为启发函数计算的距离代价 case 'taxicab'
heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); end
cost(1) = costsofar + field(newy,newx); %costsofar为之前花费的代价，field(newy,newx)为环境威胁代价，cost(1)为经过此方向点的真实代价 end
% if we can look right, we move from the left 向右查询 newx = currentpos(2) + 1; newy = currentpos(1); if newx <= n
pos(2,:) = [newy newx];
switch lower(heuristicmethod) case 'euclidean'
heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); case 'taxicab'
heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); end
cost(2) = costsofar + field(newy,newx); end
% if we can look up, we move from down 向上查询 newx = currentpos(2); newy = currentpos(1)-1; if newy > 0
pos(3,:) = [newy newx];
switch lower(heuristicmethod) case 'euclidean'
heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); case 'taxicab'
heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); end
cost(3) = costsofar + field(newy,newx); end
% if we can look down, we move from up 向下查询 newx = currentpos(2); newy = currentpos(1)+1; if newy <= n
pos(4,:) = [newy newx];
switch lower(heuristicmethod) case 'euclidean'
heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); case 'taxicab'
heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); end
cost(4) = costsofar + field(newy,newx); end
% return [row column] to linear index
posinds = sub2ind([n n],pos(:,1),pos(:,2)); %posinds为此点扩展的四个方向上的坐标
% end of this function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化界面
function [field, startposind, goalposind, costchart, fieldpointers] = ... initializeField(n,wallpercent)
% This function will create a field with movement costs and walls, a start % and goal position at random, a matrix in which the algorithm will store % f values, and a cell matrix in which it will store pointers
% create the field and place walls with infinite cost 初始化界面和墙 field = ones(n,n) + 10*rand(n,n);
% field(ind2sub([n n],ceil(n^2.*rand(floor(n*n*wallpercent),1)))) = Inf; %floor(x)下取整，即舍去正小数至最近整数，ceil(x)上取整，即加入正小数至最近整数，Inf代表正无穷
field(ceil(n^2.*rand(floor(n*n*wallpercent),1))) = Inf; %ind2sub是用来将线性坐标
(总体位置序号)
转为多维坐标(包含行列的坐标)的，发现其实不用转为多维坐标就可以，矩阵field可以访问线性坐标
% create random start position and goal position 随机选择行列作为起点与终点 startposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %sub2ind用来将行列坐标转换为线性坐标，这里是必要的，因为如果把startposind设置成[x,y]的形式，访问field([x,y])的时候
goalposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %它并不是访问x行y列元素，而是访问线性坐标为x和y的两个元素
% force movement cost at start and goal positions to not be walls 将初始坐标设置为0，以免成为墙
field(startposind) = 0; field(goalposind) = 0;
% put not a numbers (NaN) in cost chart so A* knows where to look
costchart = NaN*ones(n,n); %costchart用来存储各个点的实际代价，NaN代表不是数据（不明确的操作）
% set the cost at the starting position to be 0
costchart(startposind) = 0; %起点的实际代价 % make fieldpointers as a cell array 生成n*n的元胞
fieldpointers = cell(n,n); %fieldpointers用来存储各个点的来源方向
% set the start pointer to be "S" for start, "G" for goal 起点设置为"S",终点设置为"G"
fieldpointers{startposind} = 'S'; fieldpointers{goalposind} = 'G'; % everywhere there is a wall, put a 0 so it is not considered 墙设置为0
fieldpointers(field == Inf) = {0}; %很好的方式，field == Inf 返回墙的位置，fieldpointers(field == Inf)设置相应的位置 % end of this function
%%%%%%%%%%%%%%%%%%%%
function axishandle = createFigure(field,costchart,startposind,goalposind) % This function creates a pretty figure
% If there is no figure open, then create one
if isempty(gcbf) %gcbf是当前返回图像的句柄 f1 = figure('Position',[450 150 500 500],'Units','Normalized', ...
'MenuBar','none'); %这里的Position属性值为一个四元数组 rect = [left, bottom, width, height]，第一、二个参数表示窗口位置，都是从屏幕的左下角计算的
%normalized — Units map the lower-left corner of the figure window to (0,0) and the upper-right corner to (1.0,1.0).
Caxes2 = axes('position', [0.01 0.01 0.98 0.98],'FontSize',12, ... 'FontName','Helvetica'); %position根据前面figure设置的
单位，in normalized units where (0,0) is the lower-left corner and (1.0,1.0) is the upper-right else
% get the current figure, and clear it 获得当前图像并清空 gcf; cla; end
n = length(field);
% plot field where walls are black, and everything else is white 0是黑色 field(field < Inf) = 0; %注意，虽然修改了field，但是这里的field属于局部变量，根本没有影响主函数中的field
pcolor([1:n+1],[1:n+1],[field field(:,end); field(end,:) f
ield(end,end)]); %多了一行一列
% set the colormap for the ploting the cost and looking really nice
cmap = flipud(colormap('jet')); %flipud用于反转矩阵 colormap为产生jet类型的颜色表 jet ranges from blue to red % make first entry be white, and last be black
cmap(1,:) = zeros(3,1); cmap(end,:) = ones(3,1); %改变颜色表将尾色变为(0,0,0)是黑色，起色变为(1,1,1)是白色
% apply the colormap, but make red be closer to goal 红色是更接近目标的颜色 colormap(flipud(cmap));
% keep the plot so we can plot over it
%********不用反转就可以*********% %cmap = colormap('jet');
%cmap(1,:) = ones(3,1); cmap(end,:) = zeros(3,1); %colormap(cmap);
%*******************************%
hold on;
% now plot the f values for all tiles evaluated
axishandle = pcolor([1:n+1],[1:n+1],[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);
% plot goal as a yellow square, and start as a green circle
[goalposy,goalposx] = ind2sub([n,n],goalposind); %注意返回的列和行的位置 [startposy,startposx] = ind2sub([n,n],startposind);
plot(goalposx+0.5,goalposy+0.5,'ys','MarkerSize',10,'LineWidth',6); %加0.5是为了把坐标移到方块中央，'ys'中y表示yellow,s表示Square(方形)
plot(startposx+0.5,startposy+0.5,'go','MarkerSize',10,'LineWidth',6); %'go'中g表示green,o表示Circle(圆形) % add a button so that can re-do the demonstration
uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, ...
'Position', [1 1 60 40], 'Callback','astardemo'); % end of this function