A*算法（二）——最小堆实现

A*简介

之前写了一篇A*算法的实现文章，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*算法（一）——简单实现实现了简单的A*算法，但是效率很低，原因是：

OpenList是一个简单的向量Vector，步骤②中，从OpenList中找f值最小的节点，时间复杂度为O（n），需要浪费大量的时间。

如果使用最大堆数据结构，每次添加节点到OpenList的复杂度为O（logn），从OpenList中找最小的节点复杂度为O（1），从OpenList中展出删除元素的复杂度为O（logn），整体的复杂度可以从O（n）变为O（logn）。

下面贴出最要修改的部分代码

往OpenList中添加节点

void AStar::addNodeToOpenList(ListNode * node)
{
	int tmpF = node->F;

	openList.push_back(node);        //先把新的元素放在vector最后

	int theNewIndex = openList.size() - 1;   //新元素的下标
	while (true)
	{
		int theParentIndex = theNewIndex / 2;     //新元素当前所在位置的父节下标
		if (theParentIndex != 0)
		{
			if (openList[theNewIndex]->F < openList[theParentIndex]->F)   //新元素的F值比他的父节点值大
			{
				//与父节点的位置进行交换
				ListNode * tmpinfo = openList[theParentIndex];
				openList[theParentIndex] = openList[theNewIndex];
				openList[theNewIndex] = tmpinfo;

				openList[theParentIndex]->index = theParentIndex;
				openList[theNewIndex]->index = theNewIndex;

				theNewIndex = theParentIndex;   //置新节点的处理下标为换过位置以后的值
			}
			else
			{
				break;
			}
		}
		else                                    //如果父节下标计算出来为0，则停止查找位置
		{
			break;
		}
	}
}

从OpenList中找出f值最小的节点，并删除

ListNode * AStar::findLeastFInOpenList()
{

	if (openList.size() <= 1)   //开放列表里面只剩下开始放进去的一个空的POINTINFO指针
	{
		return NULL;
	}

	//先把最后一个元素放到1号位置
	if (openList.size() == 2)
	{
		ListNode * tmpinfo = openList[1];    //下标1位置的元素先存储起来，函数结束的时候要return出去
		openList[1] = openList.back();    //把vector中的最后一个元素放到下标1位置
		openList.pop_back();
		return tmpinfo;
	}
	ListNode * tmpinfo = openList[1];    //下标1位置的元素先存储起来，函数结束的时候要return出去
	openList[1] = openList.back();    //把vector中的最后一个元素放到下标1位置
	openList.pop_back();
	openList[1]->index = 1;

	int theSelectIndex = 1;       //要处理元素的下标值
	while (true)
	{
		int theLeftChildIndex = theSelectIndex * 2;               //左子节点下标值
		int theRightChildIndex = theSelectIndex * 2 + 1;          //右子节点下标值
		if (theLeftChildIndex >= openList.size())   //这个数没有子节点，则停止排序
		{
			break;
		}
		else if (theLeftChildIndex == (openList.size() - 1)) //左子节点正好是vector中最后一个元素，即只有左子节点，没有右子节点
		{
			if (openList[theSelectIndex]->F > openList[theLeftChildIndex]->F)   //如果父节点的F值比左子节点更大
			{
				//交换
				ListNode * tmptmpinfo = openList[theLeftChildIndex];
				openList[theLeftChildIndex] = openList[theSelectIndex];
				openList[theSelectIndex] = tmptmpinfo;

				openList[theLeftChildIndex]->index = theLeftChildIndex;
				openList[theSelectIndex]->index = theSelectIndex;

				theSelectIndex = theLeftChildIndex;
			}
			else  //如果小，则停止排序
			{
				break;
			}
		}
		else if (theRightChildIndex < openList.size())  //既有左子节点又有右子节点
		{
			if (openList[theLeftChildIndex]->F <= openList[theRightChildIndex]->F)   //左右子节点先互相比较 左边的小
			{
				if (openList[theSelectIndex]->F > openList[theLeftChildIndex]->F)      //处理的父节点F值比左子节点大
				{
					//交换(与左子节点)
					ListNode * tmptmpinfo = openList[theLeftChildIndex];
					openList[theLeftChildIndex] = openList[theSelectIndex];
					openList[theSelectIndex] = tmptmpinfo;

					openList[theLeftChildIndex]->index = theLeftChildIndex;
					openList[theSelectIndex]->index = theSelectIndex;

					theSelectIndex = theLeftChildIndex;
				}
				else
				{
					break;
				}
			}
			else     //右边的比较小
			{
				if (openList[theSelectIndex]->F > openList[theRightChildIndex]->F)      //处理的F值比右子节点大
				{
					//交换(与右子节点)
					ListNode * tmptmpinfo = openList[theRightChildIndex];
					openList[theRightChildIndex] = openList[theSelectIndex];
					openList[theSelectIndex] = tmptmpinfo;

					openList[theRightChildIndex]->index = theRightChildIndex;
					openList[theSelectIndex]->index = theSelectIndex;

					theSelectIndex = theRightChildIndex;
				}
				else
				{
					break;
				}
			}
		}

	}
	return tmpinfo;
}