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