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;
}
}