以前觉得看不懂的A*算法最近心血来潮突然想搞一下,结果看了一遍原理写完之后,发现好像也就那么回事。
参考文章:https://www.cnblogs.com/21207-iHome/p/6048969.html
总之先上算法流程:
1. 把起点加入 open list 。
2. 重复如下过程:
a. 遍历open list ,查找F值最小的节点,把它作为当前要处理的节点,然后移到close list中
b. 对当前方格的 8 个相邻方格一一进行检查,如果它是不可抵达的或者它在close list中,忽略它。否则,做如下操作:
□ 如果它不在open list中,把它加入open list,并且把当前方格设置为它的父亲
□ 如果它已经在open list中,检查这条路径 ( 即经由当前方格到达它那里 ) 是否更近。如果更近,把它的父亲设置为当前方格,并重新计算它的G和F值。
c. 遇到下面情况停止搜索:
□ 把终点加入到了 open list 中,此时路径已经找到了,或者
□ 查找终点失败,并且open list 是空的,此时没有路径。
3. 从终点开始,每个方格沿着父节点移动直至起点,形成路径。
其中提到几个名词:G,H,F
G代表起点到当前节点的代价,也可以理解为起点到当前节点要走多少路。
H代表当前节点到终点的代价但是忽略掉障碍和斜边,就等于只走直线到终点要走多少路。
F就是G+H。
也就是说只要走好算法流程,A*就可以写出来了。
#include <iostream>
#include <vector>
using namespace std;
const int width = 10; //地图的大小
const int height = 10;
const int FAIL = -99;
const int BLOCK = 1; //障碍
const int WALK = 0; //可以走的路
const int PATH = 2; //最后得出的路径
const int offseti[8]{ -1,-1,-1,0,0,1,1,1 }; //8邻域偏移量
const int offsetj[8]{ -1,0,1,-1,1,-1,0,1 };
class Node
{
public:
bool inOpenList = false; //是否在OpenList中
bool inCloseList = false; //是否在CloseList中
bool walkable = true;
int i, j;
Node *pre; //父节点 最后通过这个得到最终路径
float G = 99; //G值:起点到当前节点的代价
float H = 99; //H值:当前节点到终点的预估代价,忽略障碍物和斜边
float fn = 99; //F(n) = G+H 用来选择下一个节点
void UpdateFn() { fn = G + H; }
Node(int _i, int _j) :i(_i), j(_j) {};
Node() {};
~Node() {};
};
inline void ShowNode(Node node)
{
cout << " (" << node.i << "," << node.j << ")" << " ";
cout << " inOpen: " << node.inOpenList;
cout << " inClose: " << node.inCloseList;
cout << " node data: " << node.G << " " << node.H << " " << node.fn;
if (node.pre != nullptr)
cout << " (" << node.pre->i << "," << node.pre->j << ")";
else
cout << " pre:null";
cout << endl;
}
inline void ShowMap(int map[][width])
{
cout << endl;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if (map[i][j] == 0)
cout << " - ";
else if (map[i][j] == BLOCK)
cout << " # ";
else if (map[i][j] == PATH)
cout << " * ";
}
cout << endl;
}
}
inline void ShowVec(vector<Node*> list)
{
for (int i = 0; i < list.size(); i++)
{
ShowNode(*list[i]);
}
cout << endl;
}
//计算n1到n2的代价(8邻域)
float Cost(const Node& n1,const Node& n2)
{
//直接相邻代价为10
if ((abs(n1.i - n2.i) == 1 && n1.j == n2.j || n1.i == n2.i && abs(n1.j - n2.j)) == 1)
return 10;
//斜相邻代价为sqrt(10*10) = 14
else if ((abs(n1.i - n2.i) == 1 && abs(n1.j - n2.j)) == 1)
return 14;
else
return FAIL;
}
//通过n1得到n2的G
float GetG(const Node& n1, const Node& n2)
{
return Cost(n1, n2) + n1.G;
}
//得到H值
float GetH(const Node& cur,const Node& end)
{
return (abs(cur.i - end.i) + abs(cur.j - end.j)) * 10;
}
void AddToOpenList(Node* node, vector<Node*>& list)
{
list.push_back(node);
node->inOpenList = true;
node->inCloseList = false;
}
void AddToCloseList(Node* node, vector<Node*>& list)
{
list.push_back(node);
node->inOpenList = false;
node->inCloseList = true;
}
//在OpenList中找最小F(n)返回索引
int FindMinF_n(vector<Node*> nodes)
{
if (nodes.size() == 0)
return FAIL;
float min = nodes[0]->fn;
int idx = 0;
for (int i = 1; i < nodes.size(); i++)
{
if (nodes[i]->fn < min)
{
min = nodes[i]->fn;
idx = i;
}
}
return idx;
}
//对当前节点的8邻域节点进行操作
void GetSurroundNode(Node* node, Node* nodes[][width], vector<Node*>& openList,const Node& end)
{
for (int k = 0; k < 8; k++)
{
if ((node->i + offseti[k]) >= 0 && (node->i + offseti[k]) < height && (node->j + offsetj[k]) >= 0 && (node->j + offsetj[k]) < width)
{
//检测合法性
}
else
{
continue;
}
Node* temp = nodes[node->i + offseti[k]][node->j + offsetj[k]];
if (temp->inCloseList || temp->walkable == false)
continue;
//如果节点已经在OpenList中
if (temp->inOpenList)
{
//通过当前节点重新计算G值,如果计算出的G更小,将节点的父节点设置为当前节点
if (GetG(*node, *temp) < temp->G)
{
temp->G = GetG(*node, *temp);
temp->pre = node;
temp->UpdateFn();
}
}
//如果节点不在OpenList中,节点的父节点设置为当前节点,计算G,H,F(n)值并加入OpenList
else if (!temp->inOpenList)
{
temp->pre = node;
//cout << "temp";
temp->G = GetG(*node, *temp);
temp->H = GetH(*temp, end);
temp->UpdateFn();
AddToOpenList(temp, openList);
//ShowNode(*temp);
}
}
}
//检查是否终点已经加入OpenList,如果是表明寻路结束
bool CheckEnd(const vector<Node*>& openList, const Node& end)
{
for (int i = 0; i < openList.size(); i++)
{
if (openList[i]->i == end.i&&openList[i]->j == end.j)
return true;
}
return false;
}
//A*算法
void Astar(Node& start, Node& end, Node* nodes[][width], int map[][width])
{
vector<Node*> openList;
vector<Node*> closeList;
//将起点加入OpenList
AddToOpenList(nodes[start.i][start.j], openList);
while (!CheckEnd(openList, end))
{
//OpenList中寻找最小F(n)的节点
int minFnIdx = FindMinF_n(openList);
Node* curNode = openList[minFnIdx];
//存入closeList
AddToCloseList(curNode, closeList);
//擦除OpenList的节点
openList.erase(openList.begin() + minFnIdx);
//操作节点8邻域
GetSurroundNode(curNode, nodes, openList,end);
}
//从终点节点一路上循生成路径
Node* path = nodes[end.i][end.j];
while (path != nullptr)
{
map[path->i][path->j] = PATH;
path = path->pre;
}
}
int main()
{
//7 0
//4 8
//0 0 0 0 0 0 0 0 0 0
//0 0 0 0 0 0 0 0 0 0
//0 0 0 0 0 0 0 0 0 0
//0 0 0 0 0 0 1 0 0 0
//0 0 0 0 0 0 1 0 0 0
//0 0 1 1 1 1 1 1 1 0
//0 0 0 0 0 0 1 0 0 0
//0 0 0 0 0 0 0 0 0 0
//0 0 0 0 0 0 0 0 0 0
//0 0 0 0 0 0 0 0 0 0
int map[height][width];
cout << "input start point: " << endl;
Node *start = new Node();
cin >> start->i >> start->j;
cout << "input end point: " << endl;
Node *end = new Node();
cin >> end->i >> end->j;
Node* nodes[height][width];
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
nodes[i][j] = new Node(i, j);
//H是不会变的直接初始化即可
nodes[i][j]->H = GetH(*nodes[i][j], *end);
}
}
//设置起始节点数据
nodes[start->i][start->j]->G = 0;
nodes[start->i][start->j]->H = GetH(*start, *end);
nodes[start->i][start->j]->UpdateFn();
cout << "init map:" << endl;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
cin >> map[i][j];
if (map[i][j] == BLOCK)
{
nodes[i][j]->walkable = false;
}
}
}
map[start->i][start->j] = PATH;
map[end->i][end->j] = PATH;
ShowMap(map);
Astar(*start, *end, nodes, map);
ShowMap(map);
system("pause");
}
当然这只是最简单的算法实现,如果要真正用的话还有许多地方要进行优化。