简易优化后的A*算法

A*算法优化过程

第一步，将点是否在open、close链表中的判断方式使用的数据结构更换为数组，利用空间换时间，在空间和时间之间进行折中，使用一字节中的两位分别代表一个元素是否在open、close链表中，如果内存足够可以进一步优化。

第二步，优化在open表中取最小值的方式，这里使用MuiltSet来实现自动排序，取最小元素时间O（1）。

第三步，地图预处理，使用地图整形数组的一个整形中的第八位表示四周可以展开的点，减小后期查找时间。

 

效率分析

寻路（限制寻路距离350），静态物体数量为25%，人物为200+（少于300），单次统计寻路此时平均90次左右。

地图尺寸128*128

在较小尺寸地图内，A*算法查找非常高效，单次统计总耗时为0.05ms左右。

 

地图尺寸1000*1000

在增大地图尺寸之后，A*算法效率有所下降，但是在1000*1000尺寸规模单次统计做90次左右寻路的耗时还是在0.5s左右。

 

5000*5000尺寸地图

在5000*500尺寸的地图，单次统计100次左右寻路的耗时变得比较高。

 

10000*10000尺寸地图

A*算法寻路变得非常吃力，单次寻路（限制距离350）耗时为450ms左右。由此可以得出结论A*算法效率是跟内存消耗具有很大关系，经常性读取内存不连续的变量操作将非常耗费性能，在大尺寸地图需要考虑其他方向的优化，比如预存路径，将地图分块等操作。

 

 

代码

#ifndef __ASTART_H__
#define __ASTART_H__
/********************************
**	A*算法对象类
**  by yys 2019年3月25日15:40:59
********************************/
#include "RedefineType.h"
#include 
#include 
#include 



struct Point
{
	__int16 _x, _y;				//点坐标，这里为了方便按照C++的数组来计算，_x代表横排，_y代表竖列
	int _F, _G, _H;				//_F=_G+_H
	Point *_parent;				//_parent指针，指向父节点
	Point(int x, int y) :_x(x), _y(y), _F(0), _G(0), _H(0), _parent(NULL) {}	//变量初始化
	Point() :_x(0), _y(0), _F(0), _G(0), _H(0), _parent(NULL) {}				//默认构造函数
	bool operator==(Point &p) { return p._x == _x && p._y == _y; }				//重载等于
};
struct CmpPoint					//set Point*的比较函数
{
	bool operator()(const Point* p1, const Point* p2) const
	{
		return p1->_F < p2->_F;
	}
};
class Person;
class Astar
{
	//重命名类型 简化长度
	using MSetPoint = std::multiset;
	using VVint8 = std::vector>;
	using Vint8 = std::vector<__int8>;
	using VVPPoint = std::vector>;
	using VPPoint = std::vector;
public:
	Astar(bool isIgnoreCorner);
	~Astar();
	void InitAstar(VVINT &maze);
	std::list GetPath(Point &startPoint, Point &endPoint);
	VVINT& GetMap() {return _maze;}
	//关于交互
	void AddPerson(const Person *person);
	void MovePerson(const Point *start, const Point* target);
	void DeletePerson(const Person *person);
	bool IsPointCanreach(int px2, int py2) const;
private:
	Point *FindPath(Point &startPoint, Point &endPoint);
	Point *GetEndPoint(Point* endPoint);
	std::vector GetSurroundPoints(VVint8& openCloseList, const Point *point) const;
	bool IsPointCanreachTarget(int px1, int py1, int px2, int py2, bool isIgnoreCorner) const;
	bool IsInList(VVint8& openCloseList, bool listType, Point *point) const;
	void AddPointToOpenSet(VVint8& openCloseArray,Point *point);
	void MoveOpenPointToClose(VVint8& openCloseArray,Point *point);
	//计算FGH值
	int CalcG(Point *temp_start, Point *point);
	int CalcH(Point *point, Point *end);
	int CalcF(Point *point);

private:
	bool _isIgnoreCorner;		//是否忽视拐角
	int _height;				//地图宽高
	int _width;
	VVINT _maze;				//地图
	MSetPoint _openSets;		//开表集合
	VVPPoint _points;			//点集
};
#endif // !__ASTART_H__

#include "Astar.h"
#include "Person.h"
#include 

const int COST1 = 10;			//直移一格消耗
const int COST2 = 14;			//斜移一格消耗

enum List_Type { CLOSE_LIST, OPEN_LIST };
const int direction[2][8] = { {-1,-1,-1,0,0,1,1,1},{-1,0,1,-1,1,-1,0,1} };

Astar::Astar(bool isIgnoreCorner) :_isIgnoreCorner(isIgnoreCorner)
{

}

Astar::~Astar()
{
	for (auto &rows : _points)
		for (auto &point : rows)
			delete point;
}

void Astar::InitAstar(VVINT &maze)
{
	_maze = std::move(maze);
	_height = int(_maze.size());
	_width = int(_maze[0].size());
	_points = VVPPoint(_height, VPPoint(_width, 0));

	for (int i = 0; i < _height; i++)
	{
		for (int j = 0; j < _width; j++)
		{
			//初始化点
			_points[i][j] = new Point(i, j);
			//预处理可展开方向 使用整形中的八位分别表示
			for (int k = 0; k < 8; k++)
			{
				int x = i + direction[0][k];
				int y = j + direction[1][k];
				if (IsPointCanreachTarget(i, j, x, y, _isIgnoreCorner))
				{
					_maze[i][j] |= 1 << k;				//使用低八位
					//_maze[i][j] |= 1 << (k+8);		//使用高八位
				}
			}
		}
	}
}

bool Astar::IsPointCanreach(int px2, int py2)const
{
	if (px2<0 || px2>_height - 1
		|| py2<0 || py2>_width - 1					//超出范围
		|| _maze[px2][py2] >= 1 << 8)				//障碍物
		return false;
	return true;
}

bool Astar::IsPointCanreachTarget(int px1, int py1, int px2, int py2, bool isIgnoreCorner) const
{
	if (!IsPointCanreach(px2,py2))
		return false;
	if (abs(px1 - px2) + abs(py1 - py2) == 1)	//非斜角可以
		return true;
	else
	{
		//斜对角要判断是否绊住
		if (_maze[px1][py2] < 1 << 8 && _maze[px2][py1] < 1 << 8)
			return true;
		else
			return isIgnoreCorner;	//忽略拐角返回true表示可以到达 不忽略拐角返回false表示不可到达
	}
}

int Astar::CalcG(Point *temp_start, Point *point)
{
	int extraG = (abs(point->_x - temp_start->_x) + abs(point->_y - temp_start->_y)) == 1 ? COST1 : COST2;
	return temp_start->_G + extraG;
}

int Astar::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))*COST1);
	//使用曼哈顿距离 对于不能斜向移动的  减少计算量
	//return (abs(point->_x - end->_x) + abs(point->_y - end->_y))*COST1;
}

int Astar::CalcF(Point *point)
{
	return point->_G + point->_H;
}

void Astar::AddPerson(const Person * person)
{
	int x, y;
	person->GetPosition(x, y);
	_maze[x][y] |= 1 << 8;	//目标点设置不可移动
}

void Astar::MovePerson(const Point *start, const Point* target)
{
	_maze[target->_x][target->_y] |= 1 << 8;	//目标点设置不可移动
	_maze[start->_x][start->_y] &= ~(1 << 8);	//起始点设置可移动
}

void Astar::DeletePerson(const Person * person)
{
	int x, y;
	person->GetPosition(x, y);
	_maze[x][y] &= ~(1 << 8);					//起始点设置可移动
}

Point *Astar::FindPath(Point &startPoint, Point &endPoint)
{
	VVint8 _openCloseArray(VVint8(_height, Vint8(_width / 4, 0)));
	AddPointToOpenSet(_openCloseArray, _points[startPoint._x][startPoint._y]);
	while (!_openSets.empty())
	{
		auto curPoint = *_openSets.begin();					//找到_F值最小的点
		MoveOpenPointToClose(_openCloseArray, curPoint);	//从开启列表中删除
		//1,找到当前周围八个格中可以通过的格子
		auto surroundPoints = GetSurroundPoints(_openCloseArray, curPoint);
		for (auto &target : surroundPoints)
		{
			//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算_F _G _H

			if (!IsInList(_openCloseArray, OPEN_LIST, target))
			{
				target->_parent = curPoint;
				target->_G = CalcG(curPoint, target);
				target->_H = CalcH(target, &endPoint);
				target->_F = CalcF(target);
				AddPointToOpenSet(_openCloseArray, target);
			}
			//3，对某一个格子，它在开启列表中，计算_G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新_G和_F
			else
			{
				//从curPoint 到达target更近
				int tempG = CalcG(curPoint, target);
				if (tempG < target->_G)
				{
					target->_parent = curPoint;

					target->_G = tempG;
					target->_F = CalcF(target);
				}
			}
			if (IsInList(_openCloseArray, OPEN_LIST, &endPoint))
				return GetEndPoint(&endPoint); //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
		}
	}
	return NULL;
}

Point * Astar::GetEndPoint(Point * endPoint)
{
	//从开表集合中获取终点
	for (auto &x : _openSets)
		if (*x == *endPoint)
			return x;
	return nullptr;
}

std::list Astar::GetPath(Point &startPoint, Point &endPoint)
{
	Point *result = FindPath(startPoint, endPoint);
	std::list path;
	//返回路径，如果没找到路径，返回空链表
	while (result)
	{
		path.push_front(result);
		result = result->_parent;
	}
	// 清空临时开闭列表，防止重复执行GetPath导致结果异常
	_openSets.clear();
	//恢复点集的值
	for (auto &rows : _points)
		for (auto &point : rows)
		{
			point->_F = 0;
			point->_H = 0;
			point->_G = 0;
			point->_parent = NULL;
		}
	return path;
}

bool Astar::IsInList(VVint8& _openCloseList, bool isOpenList, Point *point) const
{
	//判断点是否在表中
	if (isOpenList && _openCloseList[point->_x][point->_y / 4] & (1 << (point->_y % 4)))
		return true;
	if (!isOpenList && _openCloseList[point->_x][point->_y / 4] & (1 << ((point->_y % 4) + 4)))
		return true;
	return false;
}

void Astar::AddPointToOpenSet(VVint8& openCloseArray, Point * point)
{
	//将点添加到开表中
	_openSets.insert(point);
	openCloseArray[point->_x][point->_y / 4] |= 1 << (point->_y % 4);
}

void Astar::MoveOpenPointToClose(VVint8& openCloseArray, Point * point)
{
	//将在开表中的最小点移除 并添加到闭表中
	_openSets.erase(_openSets.begin());
	openCloseArray[point->_x][point->_y / 4] &= ~(1 << (point->_y % 4));
	openCloseArray[point->_x][point->_y / 4] |= 1 << ((point->_y % 4) + 4);
}

std::vector Astar::GetSurroundPoints(VVint8& openCloseArray, const Point * point) const
{
	std::vector surroundPoints;
	//八方向搜索
	for (int i = 0; i < 8; i++)
	{
		int x = point->_x + direction[0][i];
		int y = point->_y + direction[1][i];
		//先判断是否可展开 然后再查找是否在Close表中 可展开并且不在列表则加入到集合中
		if (_maze[point->_x][point->_y] & 1 << i && !IsInList(openCloseArray, CLOSE_LIST, _points[x][y]))
			surroundPoints.push_back(_points[x][y]);
	}
	return surroundPoints;
}

测试代码

#include "Astar.h"
#include 
#include 
using INT_TYPE = __int16;//使用十六位整形表示一个地图元素值  其中低八位用于表示可展开方向
using VVINT = std::vector>;
int main()
{
    //测试地图
	VVINT testMaze = {
		{ 0, 0, X, X, X, X, X, X, X, X, X, X },
		{ 0, 0, 0, X, X, 0, X, 0, 0, 0, 0, X },
		{ X, 0, 0, X, X, 0, X, X, 0, 0, 0, X },
		{ X, 0, 0, X, 0, 0, 0, 0, 0, X, X, X },
		{ X, 0, X, X, X, X, 0, X, 0, 0, 0, X },
		{ X, 0, X, 0, 0, 0, 0, X, 0, 0, X, X },
		{ X, 0, X, 0, X, X, X, X, 0, 0, 0, X },
		{ X, 0, X, 0, 0, 0, 0, X, 0, 0, 0, X },
		{ X, 0, X, X, X, X, 0, X, 0, 0, 0, X },
		{ X, 0, X, 0, 0, 0, 0, X, 0, 0, 0, X },
		{ X, 0, 0, 0, 0, 0, 0, X, 0, 0, 0, X },
		{ X, X, X, X, X, X, X, X, X, X, 0, 0 }
	};
	std::cout << "lets show the test map" << std::endl;
	for (auto &row : testMaze)
	{
		for (auto &value : row)
		{
			if (value == 0)
				std::cout << 0 << " ";
			else
				std::cout << "X" << " ";
		}
		std::cout << std::endl;
	}
	//设置起始和结束点
	Point start(0, 0);
	Point end(testMaze.size() - 1, testMaze[0].size() - 1);
	bool isIgnoreCorner = false;
	Astar astar(isIgnoreCorner);
	astar.InitAstar(testMaze);
	//A*算法找寻路径
	std::list path;
    path = astar.GetPath(start, end);
	//测算耗时
	//CalFunUseTime([&]() { path = astar.GetPath(start, end); });
	std::cout << "show the path:" << std::endl;
	auto map = astar.GetMap();
	int stepCount = -1;
	for (auto &p : path)
	{
		std::cout << '(' << p->_x << ',' << p->_y << ')' << std::endl;
		map[p->_x][p->_y] = stepCount--;
	}
	//可视化显示
	for (auto &row : map)
	{
		for (auto &value : row)
		{
			if (value < 0)
				std::cout << std::setw(2) << -value << " ";
			else if (value >= 1 << 8)
				std::cout << "xx" << " ";
			else
				std::cout << " 0" << " ";
		}
		std::cout << std::endl;
	}
	std::cout << "wait to init the map..." << std::endl;
}