A* 算法跟 Dijkstra 算法 很像，只是在下一步搜索中心的選擇的方法不一樣。Dijkstra 算法 沒有任何干預，找離起點 “最近”的鄰居作為備選點，如果有若干個鄰居都是相同距離的話，純粹就是按照找到的順序取第一個。A*算法，找與終點最近的鄰居，作為下一個搜索中心。(不過，如果若干個鄰居與終點的距離一樣呢？)
下面的代碼是從 Dijkstra 算法 拷貝來的，四個黃色的部分是修改的。
第二個黃色部分: 找與終點最近的鄰居作為下一個搜索點。
[~, current] = min(f(:));
f=f(:)什么意思
%將矩陣f的每一列元素堆積起來，成為一個列向量，而這也是MATLAB變量的內部儲存方式。例如：
A=[-45 65;87 64;23 54];
B=A(:)
B=
-45
87
23
65
64
23
54
[min_dist, ~] = min(distanceFromStart(:));
在
Dijkstra 算法
里，這部分是：[min_dist, current] = min(distanceFromStart(:));
第三個黃色部分: 排除已經作為搜索點的鄰居再次被 min 到的可能性f(current) = Inf; 在
Dijkstra 算法
里，這部分是：distanceFromStart(current) = Inf;
第四個黃色部分是新增加的：就是為了計算每一個鄰居與終點的距離權值。只不過這里的距離權值預先計算好了放在H矩陣里，所以直需要從 H 里取值就好了。
所以第一黃色部分：定義f,計算H
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - 4) + abs(X - 8);
f = Inf(nrows,ncols);
f(start_node) = H(start_node);
H 有很多種計算方法，可以直接算兩點距離之類。
%% % set up color map for display
cmap = [1 1 1; ...% 1 - white - clear cell
0 0; ...% 2 - black - obstacle
0 0; ...% 3 - red = visited
0 1; ...% 4 - blue - on list
1 0; ...% 5 - green - start
1 0];% 6 - yellow - destination
colormap(cmap);
map = zeros(10);
% Add an obstacle
map (1:5, 7) = 2;
map(6, 2) = 5; % start_coords
map(4, 8) = 6; % dest_coords
image(1.5,1.5,map);
grid on;
axis image;
%%
nrows = 10;
ncols = 10;
start_node = sub2ind(size(map), 6, 2);
dest_node = sub2ind(size(map), 4, 8);
% Initialize distance array
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0;
%====================
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - 4) + abs(X - 8);
f = Inf(nrows,ncols);
f(start_node) = H(start_node);
%=======================
% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);
% Main Loop
while true
% Draw current map
map(start_node) = 5;
map(dest_node) = 6;
image(1.5, 1.5, map);
grid on;
axis image;
drawnow;
%====================
% Find the node with the minimum distance
[
~, current] = min(f(:)); [min_dist, ~] =
min(distanceFromStart(:));
%===================
if ((current == dest_node) || isinf(min_dist))
break;
end;
map(current) = 3;
%============
f(current)
=
Inf;
%============
[i, j] = ind2sub(size(distanceFromStart), current);
neighbor = [i-1,j;...
i+1,j;...
i,j+1;...
i,j-1]
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
(neighbor(:,2)<1) + (neighbor(:,2)>ncols )
locate = find(outRangetest>0);
neighbor(locate,:)=[]
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2))
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
map(neighborIndex(i)) = 4;
if distanceFromStart(neighborIndex(i))> min_dist + 1
distanceFromStart(neighborIndex(i)) = min_dist+1;
parent(neighborIndex(i)) = current;
f(neighborIndex(i))
=
H(neighborIndex(i));
end
end
end
end
%%
if (isinf(distanceFromStart(dest_node)))
route = [];
else
%提取路線坐標
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
% 動態顯示出路線
for k = 2:length(route) - 1
map(route(k)) = 7;
pause(0.1);
image(1.5, 1.5, map);
grid on;
axis image;
end
end