数据结构和算法——自动寻路A*算法实现

自动寻路A*算法实现

#include 
#include 
#include 
#include  //用于求起始点到终点距离，平方根函数

using namespace std;

const int kCost1 = 10; //直移一格的消耗：每走一格的位置
const int kCost2 = 14; //斜移一个的消耗：本次不实现

typedef struct Point {
	int x;	//横排:模拟地图数组上的位置
	int y;	//竖排

	int F, G, H;	//距离
	Point* parent;	//上一步走的位置
}Point;

static int* map;	//全局二维数组地图
static int lines;	//行数
static int columns;	//列数

static list openList; //开放列表
static list closeList;	//关闭列表

int mazeMap[13][13] = {	//二维数组模拟地图
	{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, 
	{ 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, }, 
	{ 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, }, 
	{ 0, 1, 0, 1, 0, 1, 2, 1, 0, 1, 0, 1, 0, }, 
	{ 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, }, 
	{ 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, }, 
	{ 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, },
	{ 2, 0, 1, 1, 0, 0, 0, 1, 0, 1, 1, 0, 2, }, 
	{ 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, },
	{ 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, },
	{ 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, },
	{ 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, }, 
	{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, } };

//1.初始化迷宫地图
void initAstarMaze(int* maze, int line, int column) {
	map = maze;
	lines = line;
	columns = column;
}

//2.初始化分配一个节点
Point* AllocPoint(int x, int y) {
	Point* tmp = new Point;
	memset(tmp, 0, sizeof(Point));	//初始化为0
	tmp->x = x;
	tmp->y = y;
	return tmp;
}

//从开放列表中返回F值最小的节点
static Point* GetLeastFPoint() {
	if (!openList.empty()) {
		Point* resPoint = openList.front();	//获取开放列表中的第一个节点

		list::const_iterator itor;  //使用迭代器遍历
		for (itor = openList.begin(); itor != openList.end(); itor++) {
			if ((*itor)->F < resPoint->F) {
				resPoint = *itor;
			}
		}
		return resPoint;
	}
	return NULL;
}

//判断节点是否在关闭或者开放列表中
static Point* isInList(const list& list, const Point* point) {
	std::list::const_iterator it;
	for (it = list.begin(); it != list.end(); it++) {
		if ((*it)->x == point->x && (*it)->y == point->y) {
			return *it;
		}
	}
	return NULL;
}

//判断节点是否可以移动
static bool isCanreach(const Point* point, const Point* target) {
	if (target->x<0 || target->x>(lines - 1)
		|| target->y<0 || target->y>(columns - 1) //判断是否越界
		||map[target->x*columns+target->y] == 1	//判断是不是障碍物
		||map[target->x * columns + target->y] == 2
		||(target->x == point->x && target->y == point->y) //不能与当前点重叠
		||isInList(closeList,target)){//判断是否是在关闭列表中的
		return false;
	}
	//判断是否是相邻的点，使用moth库中的abs绝对值
	if (abs(point->y - target->y) + abs(point->x - target->x) == 1) {
		return true;
	}
	else {
		return false;
	}
}

//获取当前位置周围可移动的节点
static vector GetSurroundPoints(const Point* point){
	vectorsurroundPoints;
	
	for (int x = point->x - 1; x <= point->x + 1; x++) {
		for (int y = point->y - 1; y <= point->y + 1; y++) {
			Point* tmp = AllocPoint(x, y);
			if (isCanreach(point, tmp)) {//判断是否是可以移动的点排除障碍物等
				surroundPoints.push_back(tmp); //将可以移动的点加入到vector中
			}
			else {
				delete tmp;
			}
		}
	}
	return surroundPoints;
}

//计算FGH的值
static int calcG(Point* star, Point* point) {
	int extraG = (abs(point->x - star->x) + abs(point->y - star->y)) == 1 ? kCost1 : kCost2;
	int parentG = (point->parent == NULL ? NULL : point->parent->G);
	return parentG + extraG;
}
static int calcH(Point* point, Point* end) {
	//求平方根
	return (int)sqrt((double)(end->x - point->x) * (double)(end->x - point->x) + (double)(end->y - point->y) * (double)(end->y - point->y)) * kCost1;
}
static int calcF(Point* point) {
	return point->G + point->H;
}

//搜索从起点到终点的最佳路径:返回终点的位置
static Point* FindePath(Point* startPoint, Point* endPoint) {
	openList.push_back(AllocPoint(startPoint->x, startPoint->y)); //将起始位置分配节点并保存到开放列表

	while (!openList.empty()) {
		//1.获取F点最小值的节点
		Point* curPoint = GetLeastFPoint();
		//2.将当前节点从开放表删除并放到关闭列表中去
		openList.remove(curPoint);
		closeList.push_back(curPoint);
		//3.找到当前节点周围的可达的节点并计算F值
		vectorsurroundPoints = GetSurroundPoints(curPoint);

		vector::const_iterator it;
		for (it = surroundPoints.begin(); it != surroundPoints.end(); it++) {
			Point* target = *it;
			//判断当前节点是否在开放列表中，不在则加入并设置当前节点为父节点
			Point* exist = isInList(openList, target);
			if (!exist) {
				target->parent = curPoint;
				target->G = calcG(curPoint, target);
				target->H = calcH(target, endPoint);
				target->F = calcF(target);

				openList.push_back(target);
			}else {
				int tmpG = calcG(curPoint, target);
				if (tmpG < target->G) {
					exist->parent = curPoint;
					exist->G = tmpG;
					exist->F = calcF(target);
				}
				delete target;
			}
		}
		//4.结束
		surroundPoints.clear();
		Point* resPoint = isInList(openList, endPoint);
			if (resPoint) {
				return  resPoint;
			}
	}
	return NULL;
}

//3.使用A*算法寻找路径
list AstarGetPath(Point* startPoint, Point* endPoint) {
	Point* result = FindePath(startPoint, endPoint);//获取终点位置
	
	list path;
	while (result) { //返回路径，获得逆转后的路径
		path.push_front(result);
		result = result->parent;
	}
	return path;
}

//4.清理资源
void AstraClear() {
	map = NULL;
	lines = 0;
	columns = 0;

	list::iterator it;
	for (it = openList.begin(); it != openList.end();) {
		delete* it;
		it = openList.erase(it);
	}
	for (it = closeList.begin(); it != closeList.end();) {
		delete* it;
		it = closeList.erase(it);
	}
}

//测试
void AStarTest() {
	initAstarMaze(&mazeMap[0][0], 13, 13);
	Point* start = AllocPoint(12, 4);//设置起始点和结束点
	Point* end = AllocPoint(0, 0);

	listpath = AstarGetPath(start, end);
	list::iterator it;
	for (it = path.begin(); it != path.end();it++) {
		Point* cur = *it;
		cout << "(" << cur->x << "," << cur->y << ")" << endl;
	}
	AstraClear();
}

int main(void) {

	AStarTest();

	return 0;
}