| function [path, cost] = astar(start, goal, grid) |
| % 初始化 |
| openSet = containers.Map('KeyType', 'any', 'ValueType', 'any'); |
| openSet(num2str(start)) = struct('f', 0, 'g', 0, 'parent', [], 'x', start(1), 'y', start(2)); |
| closedSet = containers.Map('KeyType', 'any', 'ValueType', 'logical', 'DefaultValue', false); |
| |
| % 开导式函数（曼哈顿距离） |
| heuristic = @(x1, y1, x2, y2) abs(x1 - x2) + abs(y1 - y2); |
| |
| % 主循环 |
| while ~isempty(openSet) |
| % 查找F值最小的节点 |
| [~, currentIdx] = min(arrayfun(@(k) openSet(k).f, keys(openSet))); |
| current = openSet(num2str(currentIdx)); |
| |
| % 假如到达目的 |
| if current.x == goal(1) && current.y == goal(2) |
| path = backtrack(current, goal); |
| return; |
| end |
| |
| % 扩展节点 |
| [x, y] = ndgrid(current.x-1:current.x+1, current.y-1:current.y+1); |
| x = x( ; |
| y = y(; |
| validMoves = (x >= 1 & x <= size(grid, 1) & y >= 1 & y <= size(grid, 2) & grid(x, y) == 0); |
| |
| for i = validMoves |
| neighbor = [x(i), y(i)]; |
| tentativeGScore = current.g + 1; |
| |
| if ~closedSet(num2str(neighbor)) || tentativeGScore < openSet(num2str(neighbor)).g |
| openSet(num2str(neighbor)) = struct(... |
| 'f', tentativeGScore + heuristic(neighbor(1), neighbor(2), goal(1), goal(2)), ... |
| 'g', tentativeGScore, ... |
| 'parent', currentIdx, ... |
| 'x', neighbor(1), ... |
| 'y', neighbor(2)); |
| end |
| end |
| |
| % 移除当前节点 |
| closedSet(num2str(currentIdx)) = true; |
| remove(openSet, num2str(currentIdx)); |
| end |
| |
| % 假如找不到路径 |
| path = []; |
| cost = inf; |
| end |
| |
| function path = backtrack(current, goal) |
| if isempty(current.parent) |
| path = [goal;]; |
| else |
| path = backtrack(openSet(num2str(current.parent)), goal); |
| path = [current.x, current.y; path]; |
| end |
| end |