【算法百题之三十九】年轻人不讲武德，5分钟学会A*算法
大家好，我是Lampard~~
很高兴又能和大家见面了，接下来准备系列更新的是算法题，一日一练，早日升仙！
今天的探讨的问题是：实战8方向A*寻路算法。
若对A*概念不熟悉的年轻人们可以先看看上面两篇博客，我觉得概念解释得很不错的。今天主要和大家一起代码实战A*，先上效果图：
（1）A*算法的主要构成
在我看来，A*寻路的构成主要由三部分组成：
- 启动列表
- 关闭列表
- 地图块结点
（1）启动列表
存放有可能将要经过的地图块。
（2）关闭列表
存放不会遍历的地图块，注意不会遍历不代表没有经过，有可能是从启动列表移除出来的。
（3）地图块结点
我们把一张大地图划分成一个个规整的地图块，我们需要定义一个结点来代表这个地图块。而地图块结点中有几个比较重要的属性：
1.自己所处的行列的位置，2.H值，H值是欧几里得距离，即从当前位置到目的点的直线距离，3。G值，从起点到当前位置产生的耗费，4.F值（路径的长度） = G + H
（2）A*算法寻路过程
- 先把起点放到启动列表中
- 在启动列表中找到路径(f)最短的格子
- 将之父节点设置为起点
- 将其周围的格子放入启动列表中（已经在开启或者关闭列表中的结点则不需要放入）
- 把该结点从启动列表移动到关闭列表
- 当在启动列表中找到终点的时候结束
（3）原理就这么简单，上代码！！！
为了方便测试，我们用一个10*10的二维数组来代表我们的地图，其中3是起点，4是终点，1是障碍物，0是可走路径
int starMap[10][10] = {
{0, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 1, 4, 0, 0, 0},
{0, 0, 0, 3, 0, 1, 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, 0, 0, 0, 0, 1, 1, 1, 1, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
};然后我们定义一下地图块结点，把刚才我们说到的几个属性定义上去。记得不要忘记parent字段，这是回溯找起点找路径的指针，很重要
struct stNode {
int row;
int col;
int f; // 路径的长度：f = g + h
int g; // 从起点到当前路径产生的耗费
int h; // 预估值欧几里得距离，即从该点到目的点的直线距离
stNode* parent; // 父节点，用于索引找起点
};
然后我们就开始写我们的main函数啦~首先循坏一波，要从我们声明的地图中找到我们的起始和开始的位置。
int main() {
// 确认开启与结束结点
int rowStart, colStart, rowEnd, colEnd;
for (int i = 0; i < 10; i++) {
for (int j = 0; j < 10; j++) {
if (starMap[i][j] == 3) {
rowStart = i;
colStart = j;
}
if (starMap[i][j] == 4) {
rowEnd = i;
colEnd = j;
}
}
}
return 0;
}然后是根据得到的位置构建我们的其实和终止结点，对于终止位置来说只需要给它赋值行列坐标就可以了。对于其实坐标我们还需要对其其他几个属性赋值。首先是g值，因为本身自己就是起点，所以起点到自己的距离就是0；然后是h值，这里我们可以用一个函数distance来计算两点之间的距离。其中ITEM_W是各自的宽高，然后简单勾股定理就可以了。之后再把f值赋值成h+g即可，起点的parent是空，也就是说找到自己为止。
int distance(int rowStart, int colStart, int rowEnd, int colEnd) {
int x1 = rowStart * ITEM_W + ITEM_W / 2;
int y1 = colStart * ITEM_W + ITEM_W / 2;
int x2 = rowEnd * ITEM_W + ITEM_W / 2;
int y2 = colEnd * ITEM_W + ITEM_W / 2;
return (int)sqrt((float)(pow((x1 - x2), 2) + pow((y1 - y2), 2)));
};
// 构建开始结点和结束结点
stNode* nodeStart = new stNode;
nodeStart->row = rowStart;
nodeStart->col = colStart;
nodeStart->g = 0;
nodeStart->h = distance(rowStart, colStart, rowEnd, colEnd);
nodeStart->f = distance(rowStart, colStart, rowEnd, colEnd);
nodeStart->parent = NULL;
stNode* nodeEnd = new stNode;
nodeEnd->row = rowEnd;
nodeEnd->col = colEnd;然后就是构建我们的启动和关闭列表。这里我们使用链表作为存储工具（实际使用二叉树会更有效率），然后再按照我们的算法第一步，把起点给放入开启列表中。
// 构建开启和关闭列表
list<stNode*> openList;
list<stNode*> closeList;
openList.push_back(nodeStart);
stNode* curNode = NULL;紧接着就是开始我们算法的过程进入循环。首先我们要确认算法的终点：这个算法有两种时候会跳出循环，其一是按照流程最后在开启列表中找到终点的时候。其二是从启动列表中找最短路径的结点的时候，若找不到则证明开启列表没有结点，也就证明根本就没有路可以过去，因此这个时候也要跳出循环。我们用getNearNode来寻找启动列表中最短路径的结点。然后用getNearListNode函数来找到当前结点前后左右，左上左下右上右下8个结点（注意算法的第4点，已经在启动和关闭列表的不要放，不然会造成死循环），找到之后把当前节点放入关闭列表中，然后用removeNode函数把它从启动列表中移除。最后再把刚才找到的附近的结点，塞进启动列表中即可。短短十数行代码则已经完成了整个A*思路。
while (!isNodeInList(openList, nodeEnd)) {
curNode = getNearNode(openList);
if (curNode == NULL) {
cout << "找不到可选路径" << endl;
break;
}
list<stNode*> nearListNode;
getNearListNode(curNode, nearListNode, openList, closeList, nodeEnd);
closeList.push_back(curNode);
removeNode(curNode, openList);
for (list<stNode*>::iterator it = nearListNode.begin(); it != nearListNode.end(); it++) {
openList.push_back(*it);
}
}现在我们来看看刚才提及的几个函数，首先是getNearNode函数。emm这个很简单嘛，就是遍历链表找出最F值最小的结点。用个指针存当前最小结点就可以了
// 拿到f路径最小的结点
stNode* getNearNode(list<stNode*> &openList) {
stNode* minNode = NULL;
int tmpF = 100000;
for (list<stNode*>::iterator it = openList.begin(); it != openList.end(); it++) {
if ((*it)->f < tmpF) {
minNode = *it;
tmpF = minNode->f;
}
}
return minNode;
}然后再看看removeNode这个方法，这个更简单，就是遍历一遍结点然后把当前结点移除即可。若迭代器的行列数和当前结点的相同，则使用erase函数移除。
// 从列表中移除当前结点
void removeNode(stNode* curNode, list<stNode*> &openList) {
for (list<stNode*>::iterator it = openList.begin(); it != openList.end(); it++) {
if ((*it)->row == curNode->row && (*it)->col == curNode->col) {
openList.erase(it);
break;
}
}
}有点难度的是getNearListNode这个方法，其一要将当前结点3*3（不包含自己）的8个结点给记录下来，而且还不能在启动和关闭链表中。我们首先可以x轴加减一找出左边和右边的结点，然后y轴加减一找出上边和下边的结点。因此我们我可以使用一个双重循环i从-1到1，j从-1到1来实现这一点。当然当i,j都=0时则代表当前结点本身，则直接跳过即可。当我们找到这8各节点之后还要对此进行筛选，首先我们要排除越界的情况，就是横轴坐标已经超出地图那肯定是不行滴。然后是判断是不是阻断点，若地图该结点位置的值为1则代表是阻断点，那么我们同样跳过。最后是判断该节点是否在启动或者关闭列表中，若是的话则跳过。所有的条件都满足的话我们就把这个结点记录下来，等待塞入启动列表中。
// 把curNod旁边3*3的8个结点且不在开启或关闭列表都放进nearNodelist中
void getNearListNode(stNode* curNode, list<stNode*> &nearNodeList, list<stNode*> &openList, list<stNode*> & closeList, stNode* endNode) {
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
if (i == 0 && j == 0) continue;
int rowTmp = curNode->row + i;
int colTmp = curNode->col + j;
// 判断是否越界
if (rowTmp < 0 || rowTmp > 9 || colTmp < 0 || colTmp > 9) continue;
// 判断是不是阻挡点
if (starMap[rowTmp][colTmp] == 1) continue;
// 在开启或者关闭列表中
stNode* nearNode = new stNode;
nearNode->row = rowTmp;
nearNode->col = colTmp;
if (isNodeInList(openList, nearNode) || isNodeInList(closeList, nearNode)) {
continue;
}
nearNode->g = curNode->g + distance(curNode->row, curNode->col, nearNode->row, nearNode->col);
nearNode->h = distance(endNode->row, endNode->col, nearNode->row, nearNode->col);
nearNode->f = nearNode->g + nearNode->h;
nearNode->parent = curNode;
nearNodeList.push_back(nearNode);
}
}
}就这样整个A*的过程已经完成，之后就是为了方便校验结果，我们可以这样输出结果：若是没有最短的路径则返回没有可选路径，若正常找到路径，则可以通过其parent的指针回溯找到起点，然后把这条路的结点都换成A输出结果
if (curNode == NULL) {
cout << "找不到可选路径" << endl;
return 0;
}
stNode* findNode = NULL;
for (list<stNode*>::iterator it = openList.begin(); it != openList.end(); it++) {
if ((*it)->row == rowEnd && (*it)->col == colEnd) {
findNode = *it;
break;
}
}
stNode* node = findNode;
while (node != NULL) {
resMap[node->row][node->col] = 'A';
node = node->parent;
}
for (int i = 0; i < 10; i++) {
for (int j = 0; j < 10; j++) {
cout << resMap[i][j] << " ";
}
cout << endl;
}
【各寻路算法代码下载链接】
A*的分享就到这里啦，年轻人要耗子尾汁！
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
