A* 算法简介

初始化 Open Set 用于记录边界，并将起点放入 Open Set
初始化 Closed Set 用于记录已经到达的位置

当 Open Set 不为空时执行如下循环：
    从 Open Set 中取出并删除第一个位置 current
    将 current 放入到 Closed Set（表示已经搜索过这个点）

    如果 current 为目标点：
        提前终止循环

    根据地图，查找 current 下一步可以到达的所有位置，记为 neighbors
    对 neighbors 的每个元素进行如下操作：
        记录其来源为 current（即可以从 current 达到 neighbors）
        将其放到 Open Set 末尾（如果已经在里面则可以忽略或覆写）

（假设存在由起点到终点的路径，则循环停止时已经搜索到终点）
从终点开始，依次访问各个位置的来源，构成由终点到起点的路径
将上面的路径逆序排列，得到由起点到终点的路径

（对于 neighbors 中的某个节点）
计算从 current 过来的代价（current 的累计代价加上这一步移动的代价）
如果它已经存在 Closed Set：
    如果从 current 过来的代价比 Closed Set 中记录的代价更低：
        将该节点从 Closed Set 中删除
        将该节点重新放入 Open Set，更新其父节点和累计代价（需要将其作为边界重新搜索，以更新其后节点的累计代价）
    否则：
        （什么也不做）
否则（不在 Open Set 中）：
    将该节点放入 Open Set，并记录其父节点和累计代价

% 采用函数句柄将启发函数设置为当前节点与目标节点的距离
heuristic = @(node) sqrt((goal.row - node.row)^2 + (goal.col - node.col)^2);

% 初始化
open = PriorityQueue;                       % 采用优先队列初始化 Open Set
open.push(start, 0);                        % 将起始点添加到 Open Set
closed = dictionary;                        % 使用 Hash 表初始化 Closed Set
while open.size > 0
    current = open.pop;                     % 从 Open Set 中弹出优先级最高（代价最低）的节点
    closed(current.key) = current;          % 将当前节点放入 Closed Set，表示已经搜索过

    if current == goal                      % 如果当前节点为目标节点，提前终止循环
        goal = current;                     % 更新目标节点的父节点和累计代价信息
        break
    end

    neighbors = map.neighbors(current);     % 根据地图获取当前节点的周围节点（同时计算了累计代价）
    for k = 1:length(neighbors)
        key = neighbors(k).key;
        % 根据累计代价和启发代价计算优先级
        priority = neighbors(k).cost + heuristic(neighbors(k));
        if isKey(closed, key)
            % 如果 Closed Set 存在当前节点（已经搜索过），根据实际代价确定是否替换
            if neighbors(k).cost < closed(key).cost
                closed(key) = [];
                open.push(neighbors(k), priority)
            end
        else
            open.push(neighbors(k), priority)
        end
    end

% 从终点开始反向索引各节点的父节点并记录
node = goal;
astar_path = zeros(goal.depth, 3);
for k = 1:goal.depth
    astar_path(k, :) = [node.row, node.col, node.cost];
    node = node.parent;
end

% 逆序，获得从起点到终点的路径
astar_path = astar_path(end:-1:1, :);