A*算法（一）——简单实现

最近准备参加海康威视的一个软件挑战赛（http://codechallenge.hikvision.com/topic_introd.aspx?k1=6），需要用到路径搜索的算法，参考了网上的一些案例，自己实现了一个简单的A*算法。

A*算法是一种启发式的路径搜索算法。对于地图中的每一个节点，我们记录起点到该节点的消耗g，估算该节点到终点的消耗h（并不是准确值，有多种估算方法，简单的比如欧氏距离），已经两者之和f=g+h。

具体步骤为：

①将起点放入OpenList；

②从OpenList中选取f值最小的节点，记为V；

③将节点V从OpenList中移除，加入CloseList中；

④遍历节点V周围的节点，记为k，判断k其是否已经加入了CloseList：

k没有加入CloseList：

k是否加入了OpenList：

是：如果其通过节点V距离更近（即V.g+distance(V,k) < k.g），记录k的父节点为V

否：将k加入OpenList，设置其父节点为V

k加入了CloseList：

无操作

⑤重复②到④，知道遇到终点；

⑥从终点寻找其父节点，直到起点，得到了终点到起点的路径。

A*算法的具体流程可以参考这些博客，讲的非常详细：

http://www.cnblogs.com/technology/archive/2011/05/26/2058842.html

http://blog.csdn.net/b2b160/article/details/4057781

其流程为

把起始格添加到 "开启列表" do { 寻找开启列表中F值最低的格子, 我们称它为当前格. 把它切换到关闭列表. 对当前格相邻的8格中的每一个 if (它不可通过 || 已经在 "关闭列表" 中) { 什么也不做. } if (它不在开启列表中) 把它添加进 "开启列表", 把当前格作为这一格的父节点, 计算这一格的 FGH if (它已经在开启列表中) { if (用G值为参考检查新的路径是否更好, 更低的G值意味着更好的路径) { 把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值. } }} while( 目标格已经在 "开启列表", 这时候路径被找到) 如果开启列表已经空了, 说明路径不存在.最后从目标格开始, 沿着每一格的父节点移动直到回到起始格, 这就是路径.

以下给出具体实现的代码（这部分代码没有经过优化，运行很慢，但是能正确实现A*算法的功能）：
其中，"MapUtils.h"中定义了：二位数组ParkMap表示地图，iMapLength表示地图长度，iMapWidth表示地图宽度。
AStarUtils.h
// by 2017年5月27日20:19:49#ifndef _A_STAR_UTILS_H_#define _A_STAR_UTILS_H_#include <vector>struct Point{Point(){x = 0;y = 0;}Point(int _x, int _y) :x(_x), y(_y){}int x;// 点的横坐标，从0开始int y;// 点的纵坐标，从0开始};struct ListNode{ListNode(){ListNode(Point(0, 0), 0, 0, 0);}ListNode(Point p, int f, int g, int h) :position(p), F(f), G(g), H(h), parentPosition(0, 0), hasParent(false){}Point position;int F = 0;// F = G + Hint G = 0;// 起点到该店的移动代价（已经走过的路程）int H = 0;// 该点到终点的估算成本（直接去横纵坐标差之和）Point parentPosition;bool hasParent;};class AStar{private:std::vector<ListNode*> openList;std::vector<ListNode*> closeList;public:// 根据输入的坐标，得到从入口到该坐标的路径// start_x ：出发点的横坐标// start_y ：出发点的纵坐标// dest_x ：目标点的横坐标// dest_y ：目标点的纵坐标// succeed ：是否成功找到路径// 返回 ：查找到的路径，但是不包括起始点//如起点为（0，0），终点为（0，1），返回路径中只会包含（0，1）。std::vector<Point> findPathInMap(int start_x, int start_y, int dest_x, int dest_y, bool & succeed);// 查找openlist中F值最小的节点ListNode * findLeastFInOpenList();// 检查一个节点是否在list中bool isNodeInOpenList(ListNode * node);bool isNodeInCloseList(ListNode * node);// 根据节点的坐标，找到对应的节点ListNode * findNodeInOpenList(Point p);ListNode * findNodeInCloseList(Point p);// 根据节点的坐标，从list中移除节点void removeNodeFromOpenList(Point p);void removeNodeFromCloseList(Point p);// 根据输入的坐标，得到从入口到该坐标的路径// dest_x ：目标点的横坐标// dest_y ：目标点的纵坐标// succeed ：是否成功找到路径// 返回 ：查找到的路径，但是不包括起始点//如起点为（0，0），终点为（0，1），返回路径中只会包含（0，1）。std::vector<Point> findPathFromEntrance(int dest_x, int dest_y, bool & succeed);// 根据输入的坐标，得到该坐标到出口的路径// start_x ：出发点的横坐标// dest_y ：出发点的纵坐标// succeed ：是否成功找到路径// 返回 ：查找到的路径，但是不包括起始点//如起点为（0，0），终点为（0，1），返回路径中只会包含（0，1）。std::vector<Point> findPathToExpot(int start_x, int dest_y, bool & succeed);};#endif

AStarUtils.cpp
// by denghaijin 2017年5月27日20:20:26#include "AStarUtils.h"#include "MapUtils.h"std::vector<Point> AStar::findPathInMap(int start_x, int start_y, int dest_x, int dest_y, bool & succeed){for (int i = 0; i < openList.size(); i++)delete openList.at(i);openList.clear();for (int i = 0; i < closeList.size(); i++)delete closeList.at(i);closeList.clear();// 1 将起点加入open listint start_g = 0;int start_h = abs(start_x - dest_x) + abs(start_y - dest_y);int start_f = start_g + start_h;openList.push_back(new ListNode(Point(start_x, start_y), start_f, start_g, start_h));// 2 遍历bool stop = false;while (!stop){// 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点ListNode * node_min_f = findLeastFInOpenList();if (node_min_f == NULL)break;// 把这个节点移到 close listListNode * tmp_node = new ListNode(node_min_f->position, node_min_f->F, node_min_f->G, node_min_f->H);tmp_node->parentPosition = node_min_f->parentPosition;tmp_node->hasParent = node_min_f->hasParent;closeList.push_back(tmp_node);removeNodeFromOpenList(node_min_f->position);// 对当前方格的四周方格进行如下操作// 左边的点if (node_min_f->position.x > 0){int cur_x = node_min_f->position.x - 1;int cur_y = node_min_f->position.y;if (cur_x == dest_x && cur_y == dest_y) // 到达终点{int g = node_min_f->G + 1;int h = 0 + 0;int f = g + h;ListNode * destNode = new ListNode(Point(cur_x, cur_y), f, g, h);destNode->parentPosition = node_min_f->position;destNode->hasParent = true;closeList.push_back(destNode);stop = true;break;}if (ParkMap[cur_y][cur_x].Mark == *"X"){int g = node_min_f->G + 1;int h = abs(cur_x - dest_x) + abs(cur_y - dest_y);int f = g + h;ListNode * leftNode = new ListNode(Point(cur_x, cur_y), f, g, h);if (!isNodeInCloseList(leftNode)){if (!isNodeInOpenList(leftNode)){leftNode->parentPosition = node_min_f->position;leftNode->hasParent = true;openList.push_back(leftNode);}else{ListNode * oldNode = findNodeInOpenList(leftNode->position);if (leftNode->G < oldNode->G){oldNode->parentPosition = node_min_f->position;oldNode->G = leftNode->G;oldNode->H = leftNode->H;oldNode->F = leftNode->F;oldNode->hasParent = true;}}}}}// 右边的点if (node_min_f->position.x < iMapLength - 1){int cur_x = node_min_f->position.x + 1;int cur_y = node_min_f->position.y;if (cur_x == dest_x && cur_y == dest_y) // 到达终点{int g = node_min_f->G + 1;int h = 0 + 0;int f = g + h;ListNode * destNode = new ListNode(Point(cur_x, cur_y), f, g, h);destNode->parentPosition = node_min_f->position;destNode->hasParent = true;closeList.push_back(destNode);stop = true;break;}if (ParkMap[cur_y][cur_x].Mark == *"X"){int g = node_min_f->G + 1;int h = abs(cur_x - dest_x) + abs(cur_y - dest_y);int f = g + h;ListNode * rightNode = new ListNode(Point(cur_x, cur_y), f, g, h);if (!isNodeInCloseList(rightNode)){if (!isNodeInOpenList(rightNode)){rightNode->parentPosition = node_min_f->position;rightNode->hasParent = true;openList.push_back(rightNode);}else{ListNode * oldNode = findNodeInOpenList(rightNode->position);if (rightNode->G < oldNode->G){oldNode->parentPosition = node_min_f->position;oldNode->G = rightNode->G;oldNode->H = rightNode->H;oldNode->F = rightNode->F;oldNode->hasParent = true;}}}}}// 上边的点if (node_min_f->position.y > 0){int cur_x = node_min_f->position.x;int cur_y = node_min_f->position.y - 1;if (cur_x == dest_x && cur_y == dest_y) // 到达终点{int g = node_min_f->G + 1;int h = 0 + 0;int f = g + h;ListNode * destNode = new ListNode(Point(cur_x, cur_y), f, g, h);destNode->parentPosition = node_min_f->position;destNode->hasParent = true;closeList.push_back(destNode);stop = true;break;}if (ParkMap[cur_y][cur_x].Mark == *"X"){int g = node_min_f->G + 1;int h = abs(cur_x - dest_x) + abs(cur_y - dest_y);int f = g + h;ListNode * upNode = new ListNode(Point(cur_x, cur_y), f, g, h);if (!isNodeInCloseList(upNode)){if (!isNodeInOpenList(upNode)){upNode->parentPosition = node_min_f->position;upNode->hasParent = true;openList.push_back(upNode);}else{ListNode * oldNode = findNodeInOpenList(upNode->position);if (upNode->G < oldNode->G){oldNode->parentPosition = node_min_f->position;oldNode->G = upNode->G;oldNode->H = upNode->H;oldNode->F = upNode->F;oldNode->hasParent = true;}}}}}// 下边的点if (node_min_f->position.y < iMapWidth - 1){int cur_x = node_min_f->position.x;int cur_y = node_min_f->position.y + 1;if (cur_x == dest_x && cur_y == dest_y) // 到达终点{int g = node_min_f->G + 1;int h = 0 + 0;int f = g + h;ListNode * destNode = new ListNode(Point(cur_x, cur_y), f, g, h);destNode->parentPosition = node_min_f->position;destNode->hasParent = true;closeList.push_back(destNode);stop = true;break;}if (ParkMap[cur_y][cur_x].Mark == *"X"){int g = node_min_f->G + 1;int h = abs(cur_x - dest_x) + abs(cur_y - dest_y);int f = g + h;ListNode * downNode = new ListNode(Point(cur_x, cur_y), f, g, h);if (!isNodeInCloseList(downNode)){if (!isNodeInOpenList(downNode)){downNode->parentPosition = node_min_f->position;downNode->hasParent = true;openList.push_back(downNode);}else{ListNode * oldNode = findNodeInOpenList(downNode->position);if (downNode->G < oldNode->G){oldNode->parentPosition = node_min_f->position;oldNode->G = downNode->G;oldNode->H = downNode->H;oldNode->F = downNode->F;oldNode->hasParent = true;}}}}}//判断是否结束// 终点加入openlist//if (findNodeInOpenList(Point(dest_x, dest_y)) != NULL)//stop = true;// 查找终点失败，并且 open list 是空的，此时没有路径// ...}// tempstd::vector<Point> reverseList;ListNode * curNode = findNodeInCloseList(Point(dest_x, dest_y));while (curNode != NULL && curNode->position.x != start_x || curNode->position.y != start_y){reverseList.push_back(curNode->position);curNode = findNodeInCloseList(curNode->parentPosition);}std::vector<Point> list;for (int index = reverseList.size() - 1; index >= 0; index--){list.push_back(reverseList.at(index));}return list;}ListNode * AStar::findLeastFInOpenList(){ListNode * res_node = NULL;int minF = INT_MAX;std::vector<ListNode*>::iterator ite;for (ite = openList.begin(); ite != openList.end(); ite++){ListNode * pNode = *ite;if (pNode->F < minF){res_node = pNode;minF = pNode->F;}}return res_node;}bool AStar::isNodeInOpenList(ListNode * node){std::vector<ListNode*>::iterator ite;for (ite = openList.begin(); ite != openList.end(); ite++){ListNode * pNode = *ite;if (pNode->position.x == node->position.x &&pNode->position.y == node->position.y){return true;}}return false;}bool AStar::isNodeInCloseList(ListNode * node){std::vector<ListNode*>::iterator ite;for (ite = closeList.begin(); ite != closeList.end(); ite++){ListNode * pNode = *ite;if (pNode->position.x == node->position.x &&pNode->position.y == node->position.y){return true;}}return false;}ListNode * AStar::findNodeInOpenList(Point p){std::vector<ListNode*>::iterator ite;for (ite = openList.begin(); ite != openList.end(); ite++){ListNode * pNode = *ite;if (pNode->position.x == p.x &&pNode->position.y == p.y){return pNode;}}return NULL;}ListNode * AStar::findNodeInCloseList(Point p){std::vector<ListNode*>::iterator ite;for (ite = closeList.begin(); ite != closeList.end(); ite++){ListNode * pNode = *ite;if (pNode->position.x == p.x &&pNode->position.y == p.y){return pNode;}}return NULL;}void AStar::removeNodeFromOpenList(Point p){std::vector<ListNode*>::iterator ite;for (ite = openList.begin(); ite != openList.end(); ite++){ListNode * pNode = *ite;if (pNode->position.x == p.x &&pNode->position.y == p.y){openList.erase(ite);break;}}}void AStar::removeNodeFromCloseList(Point p){std::vector<ListNode*>::iterator ite;for (ite = closeList.begin(); ite != closeList.end(); ite++){ListNode * pNode = *ite;if (pNode->position.x == p.x &&pNode->position.y == p.y){closeList.erase(ite);break;}}}