unity3d:Astar寻路，A星，A*，二叉堆优化Open表

原理视频

油管：https://youtu.be/i0x5fj4PqP4
别人的B站翻译：https://www.bilibili.com/video/BV1v44y1h7Dt?spm_id_from=333.999.0.0

基本概念

3个数值

G:起点到当前点步数：会随着路径的改变，例如找到一条更好的路了，A的点G = 原本A的G 和 cur路径到A的G（cur的G + cur到A的G）中的最小值

H：当前点到终点步数，乐观估计，无视障碍物。即实际的H（包含绕开障碍物的整条代价） > 按照无障碍物走的最短H

F：G+H
就是每一步都找最小F的走

每步移动代价

方便计算 * 10

Open表与Close表

var listOpen = new List() { startNode }; //开放集合,可以进行计算(但没有被选中)的节点 ,将初始节点加入到OpenSet当中。所有可能，每走一步都要放入
var listClose = new List();//封闭集合,所有已经计算过而且也被算中的节点 。表示改点已经走过，且是代价最小的点

伪代码

https://blog.csdn.net/a591243801/article/details/80655416

Init
得到StartNode与TargetNode

初始化OpenSet与CloseSet
    将初始节点加入到OpenSet当中

Loop (OpenSet.Count > 0)
//找出OpenSet当中fCost最小的点,这个点就是被选中的将要行走的下一个点
currentNode = OpenSet当中fCost最小的点

//将这个点从OpenSet当中移除
OpenSet.Remove(currentNode);
//将这个点加入到ClostSet当中
ClosrSet.Add(currentNode);

if(currentNode == targetNode)
    路径已经找到

//若路径没有找到,则查找当前选中的节点的邻居节点,
//计算他们的fCost并加入到OpenSet当中方便下一轮计算
foreach neighbour of the currentNode
    //若这个节点是不可以行走,或者已经被算中过了,则跳过这个节点
    if (!neighbour.walkable || closeSet.Contains(neighbour))
        continue;

    //计算这个这个邻居节点的gCost,若gCost更小则说明
    //通过currentNode到这个节点的距离更加的短(代价更小)
    newGCostToNeighbour = currentNode.gCost + 
                    GetDistance(currentNode,neighbour);

    //gCost若更小的话则需要重新计算这个点的fCost和他的父亲节点
    //若这个节点不在OpenSet的话,则计算这个节点的fCost与设置父亲节点
    //通过设置父亲节点,那么在之后找到路径之后可以通过父亲节点找到整条路径
    if(newGCostToNeighbour < neighbour.gCost || 
                    !openSet.Contans(neighbour)){

        neighour.fCost = evaluateFCost(neighour);
        neighour.parentNode = currentNode;

        if(!OpenSet.Contains(neighour)){
            OpenSet.Add(neighbour);
        }
    }

核心代码

节点

 public abstract class NodeBase : MonoBehaviour {
         public List<NodeBase> Neighbors { get; protected set; }//周围的邻居
        public NodeBase Connection { get; private set; }//上一个节点
        public float G { get; private set; } //起点到当前：每次找邻居会更新, 取当前到这个邻居，和原本设定中的最小
        public float H { get; private set; }//当前到达终点：乐观估计，会绕过障碍物，可能比真实到达的要小
        public float F => G + H; //两者之和

寻路

public static List<NodeBase> FindPath(NodeBase startNode, NodeBase targetNode) {
            var listOpen = new List<NodeBase>() { startNode }; //开放集合,可以进行计算(但没有被选中)的节点 ,将初始节点加入到OpenSet当中。所有可能，每走一步都要放入
            var listClose = new List<NodeBase>();//封闭集合,所有已经计算过而且也被算中的节点 。表示改点已经走过，且是代价最小的点

            while (listOpen.Any()) {

                //找到最小F todo：优化，每次节点多
                var current = listOpen[0];

                //找出OpenSet当中fCost最小的点,这个点就是被选中的将要行走的下一个点，如果F总和相同，找H最小，即到达终点最快（H是乐观估计，不算障碍物估计）
                foreach (var t in listOpen) 
                    if (t.F < current.F || ( t.F == current.F && t.H < current.H)) current = t;

                //将这个点从OpenSet当中移除
                listOpen.Remove(current);

                //将这个点加入到ClostSet当中
                listClose.Add(current);
                
                
                current.SetColor(ClosedColor);

                //路径已经到了
                if (current == targetNode)
                {
                    var currentPathTile = targetNode;
                    var path = new List<NodeBase>();
                    //var count = 100;
                    while (currentPathTile != startNode) {
                        path.Add(currentPathTile);
                        currentPathTile = currentPathTile.Connection; //cur的上一个连接点
                        //count--;
                        //if (count < 0) throw new Exception();
                        //Debug.Log("sdfsdf");
                    }
                    
                    foreach (var tile in path) tile.SetColor(PathColor);
                    startNode.SetColor(PathColor);
                    Debug.Log(path.Count);
                    return path;
                }

                //找curNode的邻居，可到达和未在close表（即已经算过的不会再进行计算）
                foreach (var neighbor in current.Neighbors.Where(t => t.Walkable && !listClose.Contains(t))) {
                    //是否未计算过这个邻居
                    var inSearch = listOpen.Contains(neighbor);
                    //从cur到邻居的G = curG+cur到邻居的G代价
                    var costToNeighbor = current.G + current.GetDistance(neighbor);

                    //路径发生改变，g会变化，例如找到一条更好的路
                    if (!inSearch || costToNeighbor < neighbor.G) {
                        neighbor.SetG(costToNeighbor);
                        neighbor.SetConnection(current);
                        
                        //h是乐观估计，只需要算一遍
                        if (!inSearch) {
                            neighbor.SetH(neighbor.GetDistance(targetNode));
                            listOpen.Add(neighbor);
                            neighbor.SetColor(OpenColor);
                        }
                    }
                }
            }
            return null;
        }

优化

Open表用二叉堆

二叉堆原理
https://www.cnblogs.com/alimayun/p/12779640.html
父节点就是下标为i/2的节点。
插入节点(按照小堆为例子)

1. 插入到最后一个位置，元素为a
2. a跟父节点b比，如果插入的a更小，上浮，ab交换位置
   删除节点
3. 删除的是头节点
4. 把最后一个临时a补到堆顶
5. 跟左b右c比较，如果a比bc中最小的还小，a与其一换位置，为下沉操作

using System;
using System.Collections.Generic;
using System.Text;

namespace DataStructure
{
    public enum HeapType
    {
        MinHeap,
        MaxHeap
    }

    public class BinaryHeap<T> where T : IComparable<T>
    {
        public List<T> items;

        public HeapType HType { get; private set; }

        public T Root
        {
            get { return items[0]; }
        }

        public BinaryHeap(HeapType type)
        {
            items = new List<T>();
            this.HType = type;
        }

        public bool Contains(T data)
        {
            return items.Contains(data);
        }

        

        public void Push(T item)
        {
            items.Add(item);

            int i = items.Count - 1;

            bool flag = HType == HeapType.MinHeap;

            while (i > 0)
            {
                if ((items[i].CompareTo(items[(i - 1) / 2]) > 0) ^ flag)
                {
                    T temp = items[i];
                    items[i] = items[(i - 1) / 2];
                    items[(i - 1) / 2] = temp;
                    i = (i - 1) / 2;
                }
                else
                    break;
            }
        }

        private void DeleteRoot()
        {
            int i = items.Count - 1;

            items[0] = items[i];
            items.RemoveAt(i);

            i = 0;

            bool flag = HType == HeapType.MinHeap;

            while (true)
            {
                int leftInd = 2 * i + 1;
                int rightInd = 2 * i + 2;
                int largest = i;

                if (leftInd < items.Count)
                {
                    if ((items[leftInd].CompareTo(items[largest]) > 0) ^ flag)
                        largest = leftInd;
                }

                if (rightInd < items.Count)
                {
                    if ((items[rightInd].CompareTo(items[largest]) > 0) ^ flag)
                        largest = rightInd;
                }

                if (largest != i)
                {
                    T temp = items[largest];
                    items[largest] = items[i];
                    items[i] = temp;
                    i = largest;
                }
                else
                    break;
            }
        }

        public T PopRoot()
        {
            T result = items[0];

            DeleteRoot();

            return result;
        }
    }

}

源码

https://github.com/luoyikun/UnityForTest
AStarDemo场景