# A*算法例子

A*算法代码下载本文涉及到的代码。

## A*算法程序代码

``````function [distance, path] = a_star(map_size, neighbors, start, goal)
% A* 算法, 找不到路径时返回 error
% map_size      输入  地图尺寸 == size(mymap)
% neighbors     输入  邻居 (cell)
% start         输入  起点
% goal          输入  目标点
% distance      输出  距离
% path          输出  路径

nodes_num = map_size(1) * map_size(2);

open_set = start;
close_set = [];
came_from = inf(nodes_num, 1);
gscore = inf(nodes_num, 1);
fscore = inf(nodes_num, 1);

gscore(start) = 0;
fscore(start) = gscore(start) + h_manhattan(map_size, start, goal);

while ~isempty(open_set)
% 取出 open_set 中 f 最小的点 current
min_f = inf;
current = 0;
for i = 1:length(open_set)
node = open_set(i);
if fscore(node) < min_f
min_f = fscore(node);
current = node;
end
end

if current == goal
distance  = gscore(current);
path = reconstruct_path(came_from, current);
return;
end

open_set = setdiff(open_set, current);
close_set = [close_set current];

for i = 1:length(neighbors{current})
neighbor = neighbors{current}(i);
if any(ismember(close_set, neighbor))
continue;
end

tentative_gscore = gscore(current) + get_distance(map_size, current, neighbor);
if ~any(ismember(open_set, neighbor))
open_set = [open_set neighbor];
elseif tentative_gscore >= gscore(neighbor)
continue;
end

came_from(neighbor) = current;
gscore(neighbor) = tentative_gscore;
fscore(neighbor) = gscore(neighbor) + h_manhattan(map_size, neighbor, goal);
end
end
error("Failure!");
end

% 返回两点曼哈顿距离
function hscore = h_manhattan(map_size, current, goal)
[current_row, current_col] = ind2sub(map_size, current);
[goal_row, goal_col] = ind2sub(map_size, goal);
hscore = abs(current_row - goal_row) + abs(current_col - goal_col);
end

% 构造路径
function path = reconstruct_path(came_from, current)
path = [current];
while came_from(current) <= length(came_from)
path = [came_from(current) path];
current = came_from(current);
end
end
``````

A*得到的路径