給定一個 n × m 的方格陣列,部分方格被阻擋(障礙物),需要從起點 start 找到一條到達終點 finish 的最短路徑。路徑只能沿上下左右四個方向移動,不能穿越障礙物。這是經典的單源最短路徑問題,適用於 BFS(廣度優先搜索)、Dijkstra、A* 等算法。本文重點講解分支限界法的隊列式實現,並對比優先隊列式優化。
f(n) = g(n) + h(n)(如曼哈頓距離),優先展開最有希望的節點,轉化為 A* 算法。當前距離 + 1,入隊。#include <iostream>
#include <queue>
#include <vector>
#include <climits>
#include <algorithm>
using namespace std;
struct Position {
int row, col;
Position(int r = 0, int c = 0) : row(r), col(c) {}
bool operator==(const Position& other) const { return row == other.row && col == other.col; }
};
// 方向向量:右、下、左、上
const Position OFFSETS[4] = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}};
/**
* 分支限界法(隊列式 BFS)求最短路徑
* @param grid 地圖:-2=牆, -1=未訪問, >=0=距離起點步數
* @param start 起點
* @param finish 終點
* @param path 輸出路徑(從起點到終點)
* @return 是否找到路徑
*/
bool FindPath(vector<vector<int>>& grid, const Position& start, const Position& finish, vector<Position>& path) {
int n = grid.size() - 2; // 實際行數 (去除上下邊界)
int m = grid[0].size() - 2; // 實際列數 (去除左右邊界)
// 邊界檢查
if (start.row < 1 || start.row > n || start.col < 1 || start.col > m ||
finish.row < 1 || finish.row > n || finish.col < 1 || finish.col > m) {
return false;
}
if (grid[start.row][start.col] == -2 || grid[finish.row][finish.col] == -2) {
return false; // 起點或終點是牆
}
if (start == finish) {
path = {start};
return true;
}
queue<Position> q;
grid[start.row][start.col] = 0; // 起點距離為 0
q.push(start);
// BFS 主循環
while (!q.empty()) {
Position here = q.front(); q.pop();
int curDist = grid[here.row][here.col];
for (const auto& offset : OFFSETS) {
Position nbr(here.row + offset.row, here.col + offset.col);
// 到達終點
if (nbr == finish) {
grid[nbr.row][nbr.col] = curDist + 1;
// 回溯構建路徑
path.clear();
Position cur = finish;
path.push_back(cur);
while (!(cur == start)) {
for (const auto& off : OFFSETS) {
Position pre(cur.row + off.row, cur.col + off.col);
if (grid[pre.row][pre.col] == grid[cur.row][cur.col] - 1) {
cur = pre;
path.push_back(cur);
break;
}
}
}
reverse(path.begin(), path.end());
return true;
}
// 可達且未訪問
if (grid[nbr.row][nbr.col] == -1) {
grid[nbr.row][nbr.col] = curDist + 1;
q.push(nbr);
}
}
}
return false; // 無路徑
}
/**
* 優先隊列式分支限界 (A* 算法)
* 使用曼哈頓距離作為啟發式函數 h(n)
*/
struct AStarNode {
Position pos;
int g; // 實際代價
int f; // g + h
AStarNode(Position p, int g_, int f_) : pos(p), g(g_), f(f_) {}
bool operator<(const AStarNode& other) const { return f > other.f; } // 小頂堆
};
bool FindPathAStar(vector<vector<int>>& grid, const Position& start, const Position& finish, vector<Position>& path) {
int n = grid.size() - 2, m = grid[0].size() - 2;
auto heuristic = [&](const Position& p) {
return abs(p.row - finish.row) + abs(p.col - finish.col);
};
vector<vector<int>> dist(grid.size(), vector<int>(grid[0].size(), INT_MAX));
vector<vector<Position>> parent(grid.size(), vector<Position>(grid[0].size(), {-1, -1}));
priority_queue<AStarNode> pq;
dist[start.row][start.col] = 0;
pq.emplace(start, 0, heuristic(start));
while (!pq.empty()) {
auto cur = pq.top(); pq.pop();
Position here = cur.pos;
if (cur.g > dist[here.row][here.col]) continue; // 過期節點
if (here == finish) break;
for (const auto& off : OFFSETS) {
Position nbr(here.row + off.row, here.col + off.col);
if (grid[nbr.row][nbr.col] == -2) continue; // 牆
int newG = cur.g + 1;
if (newG < dist[nbr.row][nbr.col]) {
dist[nbr.row][nbr.col] = newG;
parent[nbr.row][nbr.col] = here;
pq.emplace(nbr, newG, newG + heuristic(nbr));
}
}
}
if (dist[finish.row][finish.col] == INT_MAX) return false;
// 回溯路徑
path.clear();
Position cur = finish;
while (!(cur == Position(-1, -1))) {
path.push_back(cur);
cur = parent[cur.row][cur.col];
}
reverse(path.begin(), path.end());
return true;
}
// 打印地圖與路徑
void printGrid(const vector<vector<int>>& grid, const vector<Position>& path) {
int n = grid.size() - 2, m = grid[0].size() - 2;
vector<vector<char>> disp(n + 2, vector<char>(m + 2));
for (int i = 0; i <= n + 1; ++i) {
for (int j = 0; j <= m + 1; ++j) {
if (grid[i][j] == -2) disp[i][j] = '#';
else disp[i][j] = '.';
}
}
for (size_t i = 0; i < path.size(); ++i) {
disp[path[i].row][path[i].col] = (i == 0 ? 'S' : (i == path.size() - 1 ? 'E' : '*'));
}
for (int i = 0; i <= n + 1; ++i) {
for (int j = 0; j <= m + 1; ++j) cout << disp[i][j] << ' ';
cout << '\n';
}
}
int main() {
int n, m;
cout << "輸入地圖大小 n m (行 列): ";
if (!(cin >> n >> m)) return 0;
// 初始化地圖:外圍一圈牆 (-2),內部 -1
vector<vector<int>> grid(n + 2, vector<int>(m + 2, -1));
for (int i = 0; i <= n + 1; ++i) grid[i][0] = grid[i][m + 1] = -2;
for (int j = 0; j <= m + 1; ++j) grid[0][j] = grid[n + 1][j] = -2;
cout << "輸入障礙物座標 (行 列),輸入 -1 -1 結束:\n";
while (true) {
int r, c; cin >> r >> c;
if (r == -1 && c == -1) break;
if (r >= 1 && r <= n && c >= 1 && c <= m) grid[r][c] = -2;
}
cout << "輸入起點 終點 (sr sc fr fc): ";
int sr, sc, fr, fc; cin >> sr >> sc >> fr >> fc;
Position start(sr, sc), finish(fr, fc);
vector<Position> path;
// 備份地圖供 A* 使用
auto grid_bak = grid;
cout << "\n=== 隊列式分支限界 (BFS) ===\n";
if (FindPath(grid, start, finish, path)) {
cout << "找到路徑,長度: " << path.size() - 1 << endl;
printGrid(grid, path);
} else {
cout << "無路徑\n";
}
cout << "\n=== 優先隊列式分支限界 (A*) ===\n";
if (FindPathAStar(grid_bak, start, finish, path)) {
cout << "找到路徑,長度: " << path.size() - 1 << endl;
printGrid(grid_bak, path);
} else {
cout << "無路徑\n";
}
return 0;
}
原始代碼存在多處邏輯錯誤與語法問題,本實現已修正:
j+2 判斷。vector<vector<int>> 替代裸指針 int**,自動管理內存。| 算法 | 時間複雜度 | 空間複雜度 | 優缺點 |
|---|---|---|---|
| 隊列式分支限界 (BFS) | O(n×m) | O(n×m) | 保證最短路,無啟發式,搜索盲目 |
| 優先隊列式 (A*) | O(n×m log(n×m)) | O(n×m) | 啟發式引導,通常展開節點少得多,需 h(n) 可接受 |
| Dijkstra (優先隊列) | O(E log V) | O(V) | 適用帶權圖,無權圖退化為 BFS |
分支限界法為解決布線問題提供了系統性框架。隊列式(BFS)實現簡單、保證最優,適合中小規模無權圖;優先隊列式(A*)引入啟發式函數,大幅減少搜索空間,是大規模實時路徑規劃的首選。工程實踐中,建議封裝為通用的 PathFinder 組件,支持插拔式啟發式函數、動態地圖更新、多線程並行搜索,並結合 JPS (Jump Point Search) 等優化技術進一步提升性能。