几个常用寻路算法的分析与实现。
图形
采用相邻列，每个节点都有一个相邻节点集合。
无权图
1
2
3
4
5
| struct GraphNode
{
std::vector<GraphNode*> mAdjacent;
};
|
1
2
3
4
5
| struct Graph
{
std::vector<GraphNode*> mNodes;
};
|
加权图
1
2
3
4
5
6
| struct WeightedEdge
{
struct WeightedGraphNode* mFrom;
struct WeightedGraphNode* mTo;
float mWeight;
};
|
1
2
3
4
5
| struct WeightedGraphNode
{
std::vector<WeightedEdge*> mEdges;
};
|
1
2
3
4
5
| struct Graph
{
std::vector<WeightedGraphNode*> mNodes;
};
|
广度优先搜索(BFS)
首先检查起始节点一步范围内的所有节点，如果没有目标节点，再检查起始节点两步范围内的所有节点。以此类推，直至找到目标节点，或者再无有效移动为止。
在搜索期间，每一个节点都需要知道前一个被访问节点。那一个节点被称为“父节点”，该节点能够在广度优先搜索完成后，帮助重建路径。虽然可以将这些数据添加到GraphNode结构体中，但最好还是将不会变化（图形本身）的数据与其“父节点”分开。这是因为基于所选的起始和目标节点，父节点将会发生变化。分离这些数据段还意味着，如果想跨多线程同时计算多条路径，搜索就不会相互干扰。
1
| using Node2ParentMap = std::unordered_map<const GraphNode*, const GraphNode*>;
|
使用队列来进行搜索。首先，从起始节点入队，并输入一个循环。在每次迭代中，都需要出列一个节点，并入队其相邻节点。可以通过检查“父映射”，来避免多次将同一节点添加到队列中。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
| bool BFS(const Graph& graph, const GraphNode* start,
const Graph* goal, Node2ParentMap& outMap)
{
bool pathFound = false;
std::queue<cosnt GraphNode*> q;
q.emplace(start);
while (!q.empty())
{
const GraphNode* current = q.front();
q.pop();
if (current == goal)
{
pathfound = true;
break;
}
for (const GraphNode* node: current->mAdjacent)
{
const GraphNode* parent = outMap[node];
if (parent == nullptr && node != start)
{
outMap[node] = current;
q.emplace(node);
}
}
}
retrun pathFound;
}
|
如果BFS成功，就可以使用outMap中的父指针来重建路径。跟随父指针这条链条，将会产生一条目标节点到起始节点的链条，所以应采用反向搜索。
针对加权图，BFS无法保证找到最短路径。这是因为BFS根本不考虑边的权重，对于BFS，每次边的遍历都是等同的。
heuristic函数
在寻路算法中，heuristic函数是从给定节点到目标节点的估计成本。
如果heuristic函数总是小于或等于从节点x到目标节点的实际成本，那么该函数是可接受的；如果heuristic函数偶尔会高过实际成本，那么该函数便是不可接受的。这种情况下，搜索应该放弃使用该函数。
曼哈顿距离
$h(x) = |start.x - end.x| + |start.y- end.y|$
曼哈顿距离假定对角线移动是无效的，如果对角线距离有效，曼哈顿距离则会经常高估成本，使得heuristic函数不可接受
欧几里德距离
$h(x) = \sqrt{(start.x - end.x)^2 + (start.y- end.y)^2}$
贪婪最佳优先搜索(GBFS)
和BFS不同，GBFS并不是以FIFO方式使用队列来考虑节点的，而是根据启发（Heuristic）函数来决定接下来考虑哪个节点。
搜索过程中那个，GBFS并不是使用队列，而是使用两组节点。开集包含正在考虑的节点。一旦节点被选用于评估，就会被移动到闭集中。
对于开集，需要进行两个操作：
对与闭集，只需要测试成员身份，所以对闭集不需要采用实际集合。
在搜索过程中，每个节点都需要额外数据。有多个临时数据，采用结构体：
1
2
3
4
5
6
7
| struct GBFSScratch
{
const WeightedEdge* mParentEdge = nullptr;
float mHeuristic = 0.0f;
bool mInOpenSet = false;
bool mInCloseSet = false;
}
|
定义一个映射：
1
| using GBFSMap = std::unordered_map<const WeightedGraphNode*, GBFSScratch>;
|
完整代码：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
| bool GBFS(const WeightedGraph& g, const WeightedGraphNode* start,
const WeightedGraphNode* goal, GBFSMap& outMap)
{
std::vector<const WeightedGraphNode*> openSet;
const WeightedGraphNode* current = start;
outMap[current].mInCloseSet = True;
while (current != goal)
{
for (const WeightedEdge* edge: current.mEdges)
{
GBFSScratch& data = outMap[nodeTo];
if (!data.mInCloseSet)
{
data.mParentEdge = current;
if (!data.mInOpenSet)
{
data.mHeuristic = ComputeHeuristic(edge->mTo, goal);
data.mInOpenSet = True;
openSet.emplace_back(edge->mTo);
}
}
}
if (openSet.empty())
{
break;
}
auto iter = std::min_element(openSet.begin(), openSet.end(),
[&outMap](const WeightedGraphNode* a, const WeightedGraphNode* b)
{
return outMap[a].mHeuristic < outMap[a].mHeuristic;
});
current = iter;
outMap[current].mInOpenSet = false;
outMap[current].mInCloseSet = true;
openSet.erase(current)
}
return (current == goal) ? true: false;
}
|
A*搜索
A*对GBFS做了一些修改，引入了路径成本组件，该组件是从起点到给定节点的实际成本。符号$g(x)$表示节点的路径成本。A*会选择具有最低$f(x)$值的节点：
完整代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
| bool AStar(const WeightedGraph& g, const WeightedGraphNode* start,
const WeightedGraphNode* goal, AStarMap& outMap)
{
std::vector<const WeightedGraphNode*> openSet;
const WeightedGraphNode* current = start;
outMap[current].mInCloseSet = True;
while (current != goal)
{
for (const WeightedEdge* edge: current.mEdges)
{
WeightedGraphNode& nodeTo = edge->mTo;
AStarScratch& data = outMap[nodeTo];
if (!data.mInCloseSet)
{
if (!data.mInOpenSet)
{
data.mParentEdge = current;
data.mHeuristic = ComputeHeuristic(nodeTo, goal);
data.mActualFromStart = outMap[current].mActualFromStart + nodeTo.mWeight;
data.mInOpenSet = True;
openSet.emplace_back(nodeTo);
}
else {
float newG = outMap[current].mActualFromStart + nodeTo.mWeight;
if (newG < data.mActualFromStart)
{
data.mParentEdge = current;
data.mActualFromStart = newG;
}
}
}
}
if (openSet.empty())
{
break;
}
auto iter = std::min_element(openSet.begin(), openSet.end(),
[&outMap](const WeightedGraphNode* a, const WeightedGraphNode* b)
{
float fOfA = a.mActualFromStart + a.mHeuristic;
float fOfB = b.mActualFromStart + b.mHeuristic;
return fOfA < fOfB;
});
current = *iter;
openSet.erase(iter)
outMap[current].mInOpenSet = false;
outMap[current].mInCloseSet = true;
}
return (current == goal) ? true: false;
}
|