基于二叉堆优化的A星算法

本源代码借助标准C++ STL中的vector,list和heap等已封装的数据结构，优化了A星算法搜索地图、检索开始列表过程，减小了程序的时间和空间花费。经检验，检索20000*20000的随机障碍物地图时，程序在规划路径部分的平均耗时在两秒左右。

#include“stdafx.h“
#include“a_star.h“

U8 Astar::m_mtrxMapMatrix[HEIGHT][WIDTH];

U16 BitNum（U8 n）
{
	return 1<}

U8 GetBit（U16 n）
{
	assert（n != 0）;
	assert（n%2 == 0 || n == 1）;

	U8 t=0;
	while（n != 1）
	{
		t++;
		n>>=1;
	}
	return t;
}

Astar::Astar（void）:m_pntTerminal（）m_pntStart（）
{
	memset（m_mtrxMapMatrix0sizeof（m_mtrxMapMatrix））;
	m_lstPathList.clear（）;
	m_vctOpenList.clear（）;
	m_lstCloseList.clear（）;
	m_wdStep = 0;
}

Astar::~Astar（）
{
	InitOpenList（）;
	InitCloseList（）;
	m_lstPathList.clear（）;
}

MapNode* Astar::CreateNewNode（MapNode* pconst U8 dir）
{
	if（nullptr != p）
	{
		//不是开始点
		U8 t = GetBit（dir）;
		APoint pos（p->m_pntPoint+DIR_VECTOR[t]）;
		//判断点是否在地图内
		if（WithinMap（pos））
		{
			//在地图内
			U8 pv = m_mtrxMapMatrix[pos.GetX（）][pos.GetY（）];
			//判断对应位置上是否有障碍物
			if（BAR != pv）
			{
				//若无障碍物
				//依据规则创建新节点，防止出现重复
				MapNode* temp = new MapNode;
				temp->m_btValue = pv;
				temp->m_bIsVisited = false;
				temp->m_bIsReachable = true;
				temp->m_pntPoint = p->m_pntPoint+DIR_VECTOR[t];//计算新节点位置
				p->m_btSur += dir;			//标记新节点相对于源节点的位置
				if（0 == t%2）//非对角线方向 
				{
					temp->m_btSur = ~（BitNum（（t+7）%8）+BitNum（（t+9）%8）+BitNum（t））;//依据规则，该点只能探测三个方向
					temp->m_pFrom = p;
				}
				else//对角线方向
				{
					t = （t+4）%8;
					temp->m_btSur = （BitNum（（t+7）%8）+BitNum（（t+9）%8）+BitNum（t））;//依据规则，该点有三个方向不用探索
					temp->m_pFrom = p;
				}
				return temp;//返回新节点
			}
			else
			{
				//若有障碍物
				//不创建新节点
				p->m_btSur = dir;//标记障碍物相对于源节点的位置：注意，障碍物不可跨越
				if（0 != t%2）//判断是否是对角线上的障碍物
				{
					//不是则对角线方向不可通过
					p->m_btSur += BitNum（（t+7）%8）;
					p->m_btSur += BitNum（（t+9）%8）;
				}
				return nullptr;
			}
		}
		else
		{
			//不在地图内
			//不创建新节点
			p->m_btSur += dir;//标记表示已经访问过
			return nullptr;
		}
	}
	else
	{
		//是开始点
		//创建开始点
		MapNode* temp = new MapNode;
		temp->m_btValue = 0;
		temp->m_bIsVisited = false;
		temp->m_bIsReachable = true;
		temp->m_pntPoint = m_pntStart;
		temp->m_btSur = 0;	
		temp->m_pFrom = nullptr;
		return temp;//返回新节点
	}
}

void Astar::InitCloseList（void）
{
	if（!m_lstCloseList.empty（））
	{
		for（list::iterator it = m_lstCloseList.begin（）;it != m_lstCloseList.end（）;it++）
		{
			assert（*it != nullptr）;

			delete *it;
		}
		m_lstCloseList.clear（）;
	}
}

void Astar::InitOpenList（void）
{
	if（!m_vctOpenList.empty（））
	{
		for（vector::iterator it = m_vctOpenList.begin（）;it != m_vctOpenList.end（）;it++）
		{
			assert（*it != nullptr）;

			delete *it;
		}
		m_vctOpenList.clear（）;
	}
}

void Astar::SetFec（MapNode* node）
{
	SetGac（node）;
	SetHec（node）;
	node->m_fFec = node->m_fGac + node->m_fHec;
}

void Astar::SetGac（MapNode* node）
{
	float t;

	if（nullptr != node->m_pFrom）
	{
		APoint cab;
		a = （node->m_pntPoint）;
		b = （node->m_pFrom->m_pntPoint）;
		c = a - b;
		t = node->m_pFrom->m_fGac;
		t += VectorLenth（c）;
	}
	else
	{
		assert（

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2728  2017-04-07 22:30  point.h

     文件       6439  2017-04-08 13:39  a_star.cpp

     文件       3303  2017-04-08 14:14  a_star.h

     文件        690  2017-04-08 13:49  main.cpp

----------- ---------  ---------- -----  ----

                13160                    4