while any(openSet(:) > 0) % Find the minimum fScore within the open set [~, current] = min(fScore(:)); % If we've reached the goal if current == goal % Get the full path and return it final = get_path(cameFrom, current); return end % Linear index -> row, col subscripts rc = rem(current - 1, mapSize(1)) + 1; cc = (current - rc) / mapSize(1) + 1; % Remove CURRENT from openSet openSet(rc, cc) = false; % Place CURRENT in closedSet closedSet(rc, cc) = true; fScore(rc, cc) = inf; gScoreCurrent = gScore(rc, cc) + costs(rc, cc); % Get all neighbors of CURRENT. Neighbors are adjacent indices on % the map, including diagonals. % Col 1 = Row, Col 2 = Col, Col 3 = Distance to the neighbor n_ss = [ ... rc + 1, cc + 1, S2 ; ... rc + 1, cc + 0, 1 ; ... rc + 1, cc - 1, S2 ; ... rc + 0, cc - 1, 1 ; ... rc - 1, cc - 1, S2 ; ... rc - 1, cc - 0, 1 ; ... rc - 1, cc + 1, S2 ; ... rc - 0, cc + 1, 1 ; ... ]; % keep valid indices only valid_row = n_ss(:,1) >= 1 & n_ss(:,1) <= mapSize(1); valid_col = n_ss(:,2) >= 1 & n_ss(:,2) <= mapSize(2); n_ss = n_ss(valid_row & valid_col, :); % subscripts -> linear indices neighbors = n_ss(:,1) + (n_ss(:,2) - 1) .* mapSize(1); % only keep neighbors in the map and not in the closed set ixInMap = map(neighbors) & ~closedSet(neighbors); neighbors = neighbors(ixInMap); % distance to each kept neighbor dists = n_ss(ixInMap, 3); % Add each neighbor to the open set openSet(neighbors) = true; % TENTATIVE_GSCORE is the score from START to NEIGHBOR. tentative_gscores = gScoreCurrent + costs(neighbors) .* dists; % IXBETTER indicates where a better path was found ixBetter = tentative_gscores < gScore(neighbors); bestNeighbors = neighbors(ixBetter); % For the better paths, update scores cameFrom(bestNeighbors) = current; gScore(bestNeighbors) = tentative_gscores(ixBetter); fScore(bestNeighbors) = gScore(bestNeighbors) + compute_cost(mapSize, bestNeighbors, gr, gc); end % while end

这段代码是 A* 算法的核心实现部分，用于寻找两点之间的最短路径。具体来说，它依次对起点周围的格子进行探索，计算每个格子到起点的代价（gScore）以及到终点的估计代价（fScore），并将其加入 openSet 中。然后从 openSet 中选取 fScore 最小的格子进行探索，直到找到终点或 openSet 中没有格子可供探索为止。

在探索每个格子时，首先将其从 openSet 中移除并加入 closedSet 中，然后计算该格子与周围格子的代价，并将未被探索过的格子加入 openSet 中。如果发现了新的更优路径，就更新该格子到起点的代价和 fScore，并将其加入到 cameFrom 列表中，表示它是从哪个格子转移而来的。

最终，如果找到了终点，就从 cameFrom 列表中回溯路径。