利用堆结构对A*网格寻路算法进行优化

在上一篇 A*网格寻路算法

currentNode = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{ //最坏时间复杂度为 big O(openSet.Count)
   if (openSet[i].fCost < currentNode.fCost ||
      (openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost))
          currentNode = openSet[i];
}
openSet.Remove(currentNode);

这一段是算法运行最慢的部分(特别是当网格地图很大的时候)，我们通过搜索整个 openSet 试图找到 fCost 值为最小的节点。
更聪明的办法是利用小根堆结构的特性来寻找 fCost 值为最小的节点(堆顶)，这样可以把复杂度从 O(n) 降为 O(log n) 级别。

伪码

//grid.MaxSize这里用网格大小直接在编译的时候创建了静态数组
//当然也可以使用动态数组在运行的时候改变数组大小
minHeap[grid.MaxSize];//创建小根堆
minHeapCount;//堆中现有元素个数

void Add(_element)
{//在堆中插入一个新元素
    minHeap[currentItemCount] = _element;
    SortUp(_element);//保证结构一直为小根堆
    currentItemCount++;//更新数组中元素个数
}

void SortUp(_element)
{
//因为堆是一颗完全二叉树
//所以排序最坏时间复杂度 big O(minHeap.height) 也可表示为 big O(log minHeapCount)

    parentIndex = (_element.HeapIndex - 1) / 2;//获取其父节点索引

    while(true)
    {
        parentElement = minHeap[parentIndex];//根据父索引找到其节点内的元素

        //如果刚插入节点的 fCost 小于其父节点的 fCost 
        if (_element.CompareTo(parentElement ))
            Swap(_element, parentElement);//交换位置
        else
            break;//跳出循环

        parentIndex = (_element.HeapIndex - 1) / 2;//继续迭代
    }
}

int CompareTo(_nodeToCompare)
{//
    if (fCost < _nodeToCompare.fCost ||
       (fCost == _nodeToCompare.fCost && hCost < _nodeToCompare.hCost))
        return 1;
    else
        return -1;
 }

T Remove()
{//删除堆顶节点的元素
    firstItem = minHeap[0];//取出堆顶节点的元素

    //把堆尾节点的元素插入堆顶
    currentItemCount--;
    minHeap[0] = minHeap[currentItemCount];

    SortDown(minHeap[0]);//保证结构一直为小根堆

    return firstItem;
}

void SortDown(T _element)
{//时间复杂度 big O(minHeap.height)
    while(true)
    {
        int childIndexLeft = _element.HeapIndex * 2 + 1;//找到左孩子索引
        int childIndexRight = _element.HeapIndex * 2 + 2;//找到右孩子索引
        int swapIndex = 0;

        if (childIndexLeft < currentItemCount)//左索引没超出数组范围
        {   //选择左索引
            swapIndex = childIndexLeft;

            if (childIndexRight < currentItemCount)//右索引没超出数组范围
              //如果右节点中 fCost 值最小，放弃左索引，选择右索引
               if(minHeap[childIndexLeft].CompareTo(minHeap[childIndexRight]) < 0)
                    swapIndex = childIndexRight;
            //如果刚插入节点的 fCost 大于左节点或右节点的 fCost，交换两者位置
            if (_element.CompareTo(minHeap[swapIndex]) < 0)
                    Swap(_element, minHeap[swapIndex]);
            else
                return;//跳出
        }
        else
            return;//跳出
    }
}

最后贴上更改之后的伪码

void FindPath(_start,  _target)
{
     start = _start;
     target = _target;

     gCost = 0;//从起始节点到当前节点的路径的成本，
     hCost = 0;//启发值，用于估算从当前节点到目标节点成本最低的路径。
     fCost = gCost + hCost;

     List seachPath = new List();//记录最短路径的搜索规模
     List path = new List();//记录最短路径

     List closeSet = new List();//已评估的节点集合
     Heap openSet = new Heap(grid.MaxSize);//将要被评估的节点集合
     openSet.Add(start);//初始只包含 start

     while(openSet.Count > 0)//当将被估算的节点存在时，执行循环
     {
          //将 currentNode 节点从将被估算的节点中删除
          currentNode = openSet.Remove();          
          closeSet.Add(currentNode);//将 currentNode节点插入已经被估算的节点

          if(currentNode == target)
          {//如果到达终点，退出循环，并输出最短路径和搜索规模
                print(seachPath);
                print(path);
                return;
          }

           //获取 currentNode 相邻的8个方向节点
           List neighbours = GetNeighbours(currentNode);
           for (int i = 0; i < neighbours.Count; i++)
           {//循环遍历与 currentNode 相邻的节点
                neighbour = neighbours[i];

                //如果是障碍物，或者已估值，跳过
                if (!neighbour.walkable || closeSet.Contains(neighbour))
                    continue;
                else//如果以上都不是，认定为搜索规模路径
                    seachPath.Add(neighbour);

                //计算从起点到节点 neighbour 的距离
                gCostToNeighbour = currentNode.gCost + 
                                   BetweenDistance(currentNode, neighbour);

                if (gCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                {//如果起点到 neighbour 的距离小于 neighbour 的实际距离
                 //或者 neighbour 不是将被估算的节点
                     neighbour.gCost = gCostToNeighbour;//更新起始节点到当前节点距离
                     //估计 neighbour 到终点的距离
                     neighbour.hCost = BetweenDistance(neighbour, target);
                     path.Add(currentNode);//认定最短路径节点
                }

                //将 neighbour 插入将被估算的节点中
                if (!openSet.Contains(neighbour))
                    openSet.Add(neighbour);
                else//如果堆中节点数据有更新，要保证堆为小根堆
                    openSet.sortUp(neighbour);
           }
     }
}