|
搜索区域
如图所示简易地图, 其中绿色方块的是起点 (用 A 表示), 中间蓝色的是障碍物, 红色的方块 (用 B 表示) 是目的地. 为了可以用一个二维数组来表示地图, 我们将地图划分成一个个的小方块。
开始寻路
- 1.从起点A开始, 把它作为待处理的方格存入一个"开启列表", 开启列表就是一个等待检查方格的列表.
- 2.寻找起点A周围可以到达的方格, 将它们放入"开启列表", 并设置它们的"父方格"为A.
- 3.从"开启列表"中删除起点 A, 并将起点 A 加入"关闭列表", "关闭列表"中存放的都是不需要再次检查的方格
图中浅绿色描边的方块表示已经加入 "开启列表" 等待检查. 淡蓝色描边的起点 A 表示已经放入 "关闭列表" , 它不需要再执行检查.
从 "开启列表" 中找出相对最适宜的方块, 通过公式 F=G+H 来计算.
F = G + H
G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动).(耗费可根据地形、坡度、距离等设置，一般简化用距离)
H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 本文代码使用简单的欧几里得距离计算方法).
我们假设横向移动一个格子的耗费为10, 为了便于计算, 沿斜方向移动一个格子耗费是14. 为了更直观的展示如何运算 FGH, 图中方块的左上角数字表示 F, 左下角表示 G, 右下角表示 H. 看看是否跟你心里想的结果一样?
从 "开启列表" 中选择 F 值最低的方格 C (绿色起始方块 A 右边的方块), 然后对它进行如下处理:
（如果C上方和下方都是障碍物的话会走入死胡同吗？不会，根据算法，这时候C会被直接放到关闭列表，没有发生任何节点的F更新和父节点更新）
- 4.把它从 "开启列表" 中删除, 并放到 "关闭列表" 中.
- 5.检查它所有相邻并且可以到达 (障碍物和 "关闭列表" 的方格都不考虑) 的方格. 如果这些方格还不在 "开启列表" 里的话, 将它们加入 "开启列表", 计算这些方格的 G, H 和 F 值各是多少, 并设置它们的 "父方格" 为 C.
- 6.如果某个相邻方格 D 已经在 "开启列表" 里了,
检查如果用新的路径 (就是经过C 的路径) 到达它的话, G值是否会更低一些, 如果新的G值更低, 那就把它的 "父方格" 改为目前选中的方格
C, 然后重新计算它的 F 值和 G 值 (H 值不需要重新计算, 因为对于每个方块, H 值是不变的). 如果新的 G 值比较高, 就说明经过
C 再到达 D 不是一个明智的选择, 因为它需要更远的路, 这时我们什么也不做.
如图, 我们选中了 C 因为它的 F 值最小, 我们把它从 "开启列表"
中删除, 并把它加入 "关闭列表". 它右边上下三个都是墙, 所以不考虑它们. 它左边是起始方块, 已经加入到 "关闭列表" 了, 也不考虑.
所以它周围的候选方块就只剩下 4 个. 让我们来看看 C 下面的那个格子, 它目前的 G 是14, 如果通过 C 到达它的话, G将会是 10
+ 10, 这比 14 要大, 因此我们什么也不做.
然后我们继续从 "开启列表" 中找出 F 值最小的, 但我们发现 C 上面的和下面的同时为 54, 这时怎么办呢? 这时随便取哪一个都行, 比如我们选择了 C 下面的那个方块 D.
D 右边已经右上方的都是墙, 所以不考虑, 但为什么右下角的没有被加进 "开启列表" 呢? 因为如果 C 下面的那块也不可以走, 想要到达 C 右下角的方块就需要从 "方块的角" 走了, 在程序中设置是否允许这样走. (图中的示例不允许这样走)
就这样, 我们从 "开启列表" 找出 F 值最小的, 将它从 "开启列表" 中移掉, 添加到 "关闭列表". 再继续找出它周围可以到达的方块, 如此循环下去...
那么什么时候停止呢? —— 当我们发现 "开始列表" 里出现了目标终点方块的时候, 说明路径已经被找到.
输出路径
如上图所示, 除了起始方块, 每一个曾经或者现在还在 "开启列表" 里的方块, 它都有一个 "父方块", 通过 "父方块" 可以索引到最初的 "起始方块", 这就是路径.
算法伪码
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
|
把起始格添加到 "开启列表"
do
{
寻找开启列表中F值最低的格子, 我们称它为当前格.
把它切换到关闭列表.
对当前格相邻的8格中的每一个
if (它不可通过 || 已经在 "关闭列表" 中)
{
什么也不做.
}
if (它不在开启列表中)
{
把它添加进 "开启列表", 把当前格作为这一格的父节点, 计算这一格的 FGH
if (它已经在开启列表中)
{
if (用G值为参考检查新的路径是否更好, 更低的G值意味着更好的路径)
{
把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值.
}
} while( 目标格已经在 "开启列表", 这时候路径被找到)
如果开启列表已经空了, 说明路径不存在.
最后从目标格开始, 沿着每一格的父节点移动直到回到起始格, 这就是路径.
|
C++实现代码
Astar.h
|
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
|
#pragma once
#include <vector>
#include <list>
const int kCost1=10;
const int kCost2=14;
struct Point
{
int x,y;
int F,G,H;
Point *parent;
Point(int _x,int _y):x(_x),y(_y),F(0),G(0),H(0),parent(NULL)
{
}
};
class Astar
{
public:
void InitAstar(std::vector<std::vector<int>> &_maze);
std::list<Point *> GetPath(Point &startPoint,Point &endPoint,bool isIgnoreCorner);
private:
Point *findPath(Point &startPoint,Point &endPoint,bool isIgnoreCorner);
std::vector<Point *> getSurroundPoints(const Point *point,bool isIgnoreCorner) const;
bool isCanreach(const Point *point,const Point *target,bool isIgnoreCorner) const;
Point *isInList(const std::list<Point *> &list,const Point *point) const;
Point *getLeastFpoint();
int calcG(Point *temp_start,Point *point);
int calcH(Point *point,Point *end);
int calcF(Point *point);
private:
std::vector<std::vector<int>> maze;
std::list<Point *> openList;
std::list<Point *> closeList;
};
|
Astar.cpp
|
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
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
|
#include <math.h>
#include "Astar.h"
void Astar::InitAstar(std::vector<std::vector<int>> &_maze)
{
maze=_maze;
}
int Astar::calcG(Point *temp_start,Point *point)
{
int extraG=(abs(point->x-temp_start->x)+abs(point->y-temp_start->y))==1?kCost1:kCost2;
int parentG=point->parent==NULL?0:point->parent->G;
return parentG+extraG;
}
int Astar::calcH(Point *point,Point *end)
{
return sqrt((double)(end->x-point->x)*(double)(end->x-point->x)+(double)(end->y-point->y)*(double)(end->y-point->y))*kCost1;
}
int Astar::calcF(Point *point)
{
return point->G+point->H;
}
Point *Astar::getLeastFpoint()
{
if(!openList.empty())
{
auto resPoint=openList.front();
for(auto &point:openList)
if(point->F<resPoint->F)
resPoint=point;
return resPoint;
}
return NULL;
}
Point *Astar::findPath(Point &startPoint,Point &endPoint,bool isIgnoreCorner)
{
openList.push_back(new Point(startPoint.x,startPoint.y));
while(!openList.empty())
{
auto curPoint=getLeastFpoint();
openList.remove(curPoint);
closeList.push_back(curPoint);
auto surroundPoints=getSurroundPoints(curPoint,isIgnoreCorner);
for(auto &target:surroundPoints)
{
if(!isInList(openList,target))
{
target->parent=curPoint;
target->G=calcG(curPoint,target);
target->H=calcH(target,&endPoint);
target->F=calcF(target);
openList.push_back(target);
}
else
{
int tempG=calcG(curPoint,target);
if(tempG<target->G)
{
target->parent=curPoint;
target->G=tempG;
target->F=calcF(target);
}
}
Point *resPoint=isInList(openList,&endPoint);
if(resPoint)
return resPoint;
}
}
return NULL;
}
std::list<Point *> Astar::GetPath(Point &startPoint,Point &endPoint,bool isIgnoreCorner)
{
Point *result=findPath(startPoint,endPoint,isIgnoreCorner);
std::list<Point *> path;
while(result)
{
path.push_front(result);
result=result->parent;
}
return path;
}
Point *Astar::isInList(const std::list<Point *> &list,const Point *point) const
{
for(auto p:list)
if(p->x==point->x&&p->y==point->y)
return p;
return NULL;
}
bool Astar::isCanreach(const Point *point,const Point *target,bool isIgnoreCorner) const
{
if(target->x<0||target->x>maze.size()-1
||target->y<0&&target->y>maze[0].size()-1
||maze[target->x][target->y]==1
||target->x==point->x&&target->y==point->y
||isInList(closeList,target))
return false;
else
{
if(abs(point->x-target->x)+abs(point->y-target->y)==1)
return true;
else
{
if(maze[point->x][target->y]==0&&maze[target->x][point->y]==0)
return true;
else
return isIgnoreCorner;
}
}
}
std::vector<Point *> Astar::getSurroundPoints(const Point *point,bool isIgnoreCorner) const
{
std::vector<Point *> surroundPoints;
for(int x=point->x-1;x<=point->x+1;x++)
for(int y=point->y-1;y<=point->y+1;y++)
if(isCanreach(point,new Point(x,y),isIgnoreCorner))
surroundPoints.push_back(new Point(x,y));
return surroundPoints;
}
|
main.cpp
|
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
|
#include <iostream>
#include "Astar.h"
using namespace std;
int main()
{
vector<vector<int>> maze={
{1,1,1,1,1,1,1,1,1,1,1,1},
{1,0,0,1,1,0,1,0,0,0,0,1},
{1,0,0,1,1,0,0,0,0,0,0,1},
{1,0,0,0,0,0,1,0,0,1,1,1},
{1,1,1,0,0,0,0,0,1,1,0,1},
{1,1,0,1,0,0,0,0,0,0,0,1},
{1,0,1,0,0,0,0,1,0,0,0,1},
{1,1,1,1,1,1,1,1,1,1,1,1}
};
Astar astar;
astar.InitAstar(maze);
Point start(1,1);
Point end(6,10);
list<Point *> path=astar.GetPath(start,end,false);
for(auto &p:path)
cout<<'('<<p->x<<','<<p->y<<')'<<endl;
system("pause");
return 0;
|
|
客服电话
APP下载
官方微信
请发表评论