AStar 在每次主循环中都要在 openList 中找到一个 F 值最小的节点作为当前节点。之前的 openList 使用简单的数组来实现，当在其中搜寻最小节点时把整个 openList 遍历一遍找到最小的节点。这是一个可以优化的点。
为 openList 维护一个有序表
因为要在 openList 中找到最小节点，一个比较容易想到的办法是把 openList 排序，然后每次都取这个表的第一个（升序）或者最后一个（降序）节点作为当前节点。
但是，如果采用快速排序对 openList 排序的话，每次主循环内都要付出平均 O(n*log(n)) 的代价，而遍历搜索的平均代价为 O(n) 。基于这个粗略的估计，维护一个有序表似乎对提高算法速度并没有什么帮助。
使用二叉堆
二叉堆是一棵完全二叉树，它的每一个节点都小于（小顶堆）或者大于（大顶堆）它的左右儿子。
因为二叉堆的性质，找到它最小或者最大的节点花费的时间是常量的，即 O(1)。
AStar 算法在找到当前节点后还要在 openList 中移除这个节点，移除这个节点后就需要重新调整二叉堆的结构一满足它最小节点在树根的性质。这里采用的方法是将树的最后一个节点移动到树根，然后不断向分支寻找这个节点的位置，直到找到它的合适位置，这个过程的平均时间复杂度是 O(log(n))。这个过程称为“下滤”。
二叉堆在插入新的节点之后也要调整结构以满足性质。这里采用的方法是在完全二叉树的最后一个节点之后插入一个新的节点，然后不断向上调整这个节点，直到找到它的合适位置，这个过程的平均时间复杂度是 O(log(n))。这个过程称为“上滤”。
综上，在以正方形为基本节点的地图中的 AStar 算法中使用二叉堆来实现 openList 的总的时间复杂度是：
O(1) + O(log(n)) + m * O(log(n))
其中 m 是每次检测加入的新节点的个数。
下面是代码:
接口：
template <typename BinaryHeapNode>
class BinaryHeap {
public:
BinaryHeap(); //构造函数
bool empty(); //二叉堆是为空
BinaryHeapNode getMin(); //得到最小节点
BinaryHeapNode isIn(const BinaryHeapNode &node); //判断一个节点是否在二叉堆中
void insert(const BinaryHeapNode &node); //插入一个节点
void deleteMin(); //删除最小节点
private:
int _currentSize; //二叉堆的 size
std::vector<BinaryHeapNode> _array; //节点采用 vector 储存
void _percolateUp(int hole); //上滤
void _percolateDown(int hole); //下滤
};
实现：
template <typename BinaryHeapNode>
BinaryHeap<BinaryHeapNode>::BinaryHeap(){
//构造函数，初始化二叉堆的大小为 0，并为节点的储存 vector 分配一个初始尺寸
_currentSize = 0;
_array.resize(100);
}
template <typename BinaryHeapNode>
bool BinaryHeap<BinaryHeapNode>::empty(){
//判空方法
if (_currentSize == 0){
return true;
}
return false;
}
template <typename BinaryHeapNode>
BinaryHeapNode BinaryHeap<BinaryHeapNode>::isIn(const BinaryHeapNode &node){
//判断一个节点是否在这个二叉堆中，简单的遍历储存节点的 vector 来判断是否存在这个节点
//因为 vector 中储存的是 AStarNode 的指针，如果想要调用重载的 == 运算符需要对这个节点解一次引用，下同
for (int index = 1; index <= _currentSize; ++index){
if ((*node) == _array[index]){
return _array[index];
}
}
return nullptr;
}
template <typename BinaryHeapNode>
BinaryHeapNode BinaryHeap<BinaryHeapNode>::getMin(){
//返回二叉堆的第一个节点，然后从二叉堆中删除它
BinaryHeapNode min = _array[1];
deleteMin();
return min;
}
template <typename BinaryHeapNode>
void BinaryHeap<BinaryHeapNode>::insert(const BinaryHeapNode &node){
//向二叉堆中插入一个节点
//首先判断 vector 的空间是否已经用完，如果已经用完重新分配当前需要空间二倍的空间，避免频繁的调用 resize
//然后将新元素插入二叉堆的最后一个元素的后面，接着执行“上滤”操作
int hole = ++_currentSize;
if (_array.size() - 1 <= _currentSize){
_array.resize(_currentSize * 2);
}
_array[_currentSize] = node;
_percolateUp(hole);
}
template <typename BinaryHeapNode>
void BinaryHeap<BinaryHeapNode>::deleteMin(){
//删除二叉堆中最小的元素
//使用最后一个元素覆盖第一个元素，然后对第一个元素执行“下滤”操作
_array[1] = _array[_currentSize--];
_percolateDown(1);
}
template <typename BinaryHeapNode>
void BinaryHeap<BinaryHeapNode>::_percolateDown(int hole){
//下滤
int child;
BinaryHeapNode temp = _array[hole]; //先找到将要下滤的元素备用
for (; hole * 2 <= _currentSize; hole = child){ //如果这个元素不是叶节点进入循环
child = hole * 2; //获得这个元素的左儿子指针
if (child != _currentSize && (*_array[child + 1]) < _array[child]){
//如果这个元素的左儿子不是最后一个元素并且右儿子比左儿子还小，意味着如果这个元素要向下交换的话也要和右儿子交换，所以把指针移向右儿子
++child;
}
//交换操作
if ((*_array[child]) < temp){
_array[hole] = _array[child];
}else {
break;
}
}
_array[hole] = temp;
}
template <typename BinaryHeapNode>
void BinaryHeap<BinaryHeapNode>::_percolateUp(int hole){
//上滤
//不断的和当前位置的父节点做比较判断是否需要交换
for (; hole > 1 && (*_array[hole]) < _array[hole / 2]; hole /= 2){
BinaryHeapNode tempNode = _array[hole];
_array[hole] = _array[hole / 2];
_array[hole / 2] = tempNode;
}
}