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

最新推荐文章于 2024-08-16 17:31:45 发布

该-昵称已被占用

最新推荐文章于 2024-08-16 17:31:45 发布

阅读量1.1k

点赞数 1

分类专栏： C++应用笔记 C++ 算法文章标签：算法 AStar

本文链接：https://blog.csdn.net/u011091739/article/details/73198457

版权

C++应用笔记同时被 3 个专栏收录

17 篇文章 0 订阅

订阅专栏

C++

14 篇文章 0 订阅

订阅专栏

算法

4 篇文章 0 订阅

订阅专栏

最近准备参加海康威视的一个软件挑战赛（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 + H
	int 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 list
	int 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 list
		ListNode * 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 是空的，此时没有路径
		// ...
	}

	// temp
	std::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;
		}
	}
}