https://blog.csdn.net/lvcoc/article/details/86632377
原文
自己写笔记省略一点
二叉堆是堆的一种，使用完全二叉树来实现。

二叉堆

小根堆

b 非终端结点的值均不大于其左右孩子结点的值

大根堆

a 所有非终端结点的值都不小于其左右孩子的值时

我们用小根堆排序
当前节点为n 左子叶2n+1 右子叶2n+2

父节点 为（n-1）/2

新加入的元素在树的最后一个位置。然后向上和父节点做比较，如果比父节点小就交换，一直比较到根节点

插入

移除元素，就是弹出根节点，把树的最后一个位置的元素补上，向下比较排序，先比较左右子节点，得到较小的一个子节点再和自身进行比较，如果子节点小就交换。

移除

然后详细步骤看原文
下面贴出源码

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


public interface IHeapItem<T> : IComparable<T>
{
    int HeapIndex
    {
        get;
        set;
    }
}
//对二叉堆进行泛型约束
public class Heap<T> where T : IHeapItem<T>
{
    T[] items;
    /// <summary>
    /// 当前树大小
    /// </summary>
    int currentItemCount;
    /// <summary>
    /// 指定树容量
    /// </summary>
    /// <param name="maxHeapSize"></param>
    public Heap(int maxHeapSize)
    {
        items = new T[maxHeapSize];
    }
    /// <summary>
    /// 加入新元素，向上排序
    /// </summary>
    /// <param name="item"></param>
    public void Add(T item)
    {
        //先插入到最后
        item.HeapIndex = currentItemCount;
        items[currentItemCount] = item;
        //向上排序
        SortUp(item);
        currentItemCount++;
    }
    /// <summary>
    /// 移除根节点，向下排序
    /// </summary>
    /// <returns></returns>
    public T RemoveFirst()
    {
        //根节点
        T firstItem = items[0];
        currentItemCount--;
        //把最后一个元素转移到第一个元素
        items[0] = items[currentItemCount];
        items[0].HeapIndex = 0;
        SortDown(items[0]);
        return firstItem;
    }
    /// <summary>
    /// 更新树重新排序
    /// </summary>
    /// <param name="item"></param>
    public void UpdateItem(T item)
    {
        SortUp(item);
    }
    /// <summary>
    /// 返回当前元素数量
    /// </summary>
    /// <returns></returns>
    public int Count()
    {
        return currentItemCount;
    }
    /// <summary>
    /// 是否存在元素
    /// </summary>
    /// <param name="item"></param>
    /// <returns></returns>
    public bool Contains(T item)
    {
        //比较传入和找到是否一样
        return Equals(items[item.HeapIndex], item);
    }
    /// <summary>
    /// 向下排序，寻找子节点
    /// </summary>
    /// <param name="item"></param>
    void SortDown(T item)
    {
        while (true)
        {
            //左叶
            int childIndexLeft = item.HeapIndex * 2 + 1;
            //右叶
            int childIndexRight = item.HeapIndex * 2 + 2;
            int swapIndex = 0;
            //如果还存在子节点 没有子节点返回
            if (childIndexLeft < currentItemCount)
            {
                swapIndex = childIndexLeft;
                if (childIndexRight < currentItemCount)
                {
                    //a.compareto(b) a<b=-1 a=b=0 a>b=1
                    if (items[childIndexLeft].CompareTo(items[childIndexRight]) > 0)
                    {
                        swapIndex = childIndexRight;//得到小的节点
                    }
                }
                //和小的节点比较 如果子节点大返回
                //a.compareto(b) a<b=-1 a=b=0 a>b=1
                if (item.CompareTo(items[swapIndex]) > 0)
                {
                    Swap(item, items[swapIndex]);
                }
                else
                {
                    return;
                }
            }
            else
            {
                return;
            }
        }
    }
    /// <summary>
    /// 向上排序，寻找父节点
    /// </summary>
    /// <param name="item"></param>
    void SortUp(T item)
    {
        //得到父节点
        int parentIndex = (int)((item.HeapIndex - 1) *0.5f);

        while (true)
        {
            T parentItem = items[parentIndex];
            //当前节点更小就交换
            //a.compareto(b) a<b=-1 a=b=0 a>b=1
            if (item.CompareTo(parentItem)<0)
            {
                Swap(item, parentItem);
            }
            else
            {
                break;
            }
            //继续向上比较
            parentIndex= (int)((item.HeapIndex - 1) * 0.5f);
        }
    }
    /// <summary>
    /// 交换树中的位置和数据
    /// </summary>
    void Swap(T itemA, T itemB)
    {
        items[itemA.HeapIndex] = itemB;
        items[itemB.HeapIndex] = itemA;

        int itemAIndex = itemA.HeapIndex;
        itemA.HeapIndex = itemB.HeapIndex;
        itemB.HeapIndex = itemAIndex;
    }
}

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Grid : MonoBehaviour
{

    Node[,] grid;
    /// <summary>
    /// 保存网格大小
    /// </summary>
    public Vector2 gridSize;
    /// <summary>
    /// 节点半径
    /// </summary>
    public float nodeRadius;
    /// <summary>
    /// 节点直径
    /// </summary>
    float nodeDiameter;
    /// <summary>
    /// 射线图层
    /// </summary>
    public LayerMask WhatLayer;

    public Transform player;

    /// <summary>
    /// 每个方向网格数的个数
    /// </summary>
    public int gridCntX, gridCntY;

    /// <summary>
    /// 保存路径列表
    /// </summary>
    public List<Node> path = new List<Node>();
    // Use this for initialization
    void Start()
    {
        nodeDiameter = nodeRadius * 2;
        gridCntX = Mathf.RoundToInt(gridSize.x / nodeDiameter);
        gridCntY = Mathf.RoundToInt(gridSize.y / nodeDiameter);
        grid = new Node[gridCntX, gridCntY];
        CreateGrid();
    }
    /// <summary>
    ///返回地图大小的属性
    /// </summary>
    public int MaxSize
    {
        get
        {
            return gridCntX * gridCntY;
        }
    }
    private void CreateGrid()
    {
        Vector3 startPoint = transform.position - gridSize.x * 0.5f * Vector3.right
            - gridSize.y * 0.5f * Vector3.forward;
        for (int i = 0; i < gridCntX; i++)
        {
            for (int j = 0; j < gridCntY; j++)
            {
                Vector3 worldPoint = startPoint + Vector3.right * (i * nodeDiameter + nodeRadius)
                    + Vector3.forward * (j * nodeDiameter + nodeRadius);
                //此节点是否可走
                bool walkable = !Physics.CheckSphere(worldPoint, nodeRadius, WhatLayer);
                //i，j是二维数组的索引
                grid[i, j] = new Node(walkable, worldPoint, i, j);
            }
        }
    }

    public Node GetFromPos(Vector3 pos)
    {
        float percentX = (pos.x + gridSize.x * 0.5f) / gridSize.x;
        float percentY = (pos.z + gridSize.y * 0.5f) / gridSize.y;

        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);

        int x = Mathf.RoundToInt((gridCntX - 1) * percentX);
        int y = Mathf.RoundToInt((gridCntY - 1) * percentY);

        return grid[x, y];
    }
    void OnDrawGizmos()
    {
        //画出网格边缘
        Gizmos.DrawWireCube(transform.position, new Vector3(gridSize.x, 1, gridSize.y));
        //画不可走网格
        if (grid == null)
            return;
        Node playerNode = GetFromPos(player.position);
        foreach (var item in grid)
        {
            Gizmos.color = item._canWalk ? Color.white : Color.red;
            Gizmos.DrawCube(item._worldPos, Vector3.one * (nodeDiameter - 0.1f));
        }
        //画路径
        if (path != null)
        {
            foreach (var item in path)
            {
                Gizmos.color = Color.black;
                Gizmos.DrawCube(item._worldPos, Vector3.one * (nodeDiameter - 0.1f));
            }
        }
        //画玩家
        if (playerNode != null && playerNode._canWalk)
        {
            Gizmos.color = Color.cyan;
            Gizmos.DrawCube(playerNode._worldPos, Vector3.one * (nodeDiameter - 0.1f));
        }
    }

    public List<Node> GetNeibourhood(Node node)
    {
        List<Node> neibourhood = new List<Node>();
        //相邻上下左右格子
        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0)
                {
                    continue;
                }
                int tempX = node._gridX + i;
                int tempY = node._gridY + j;

                if (tempX < gridCntX && tempX > 0 && tempY > 0 && tempY < gridCntY)
                {
                    neibourhood.Add(grid[tempX, tempY]);
                }
            }
        }
        return neibourhood;
    }
}

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Node :IHeapItem<Node>{
    /// <summary>
    /// 是否可以通过此路径
    /// </summary>
    public bool _canWalk;
    /// <summary>
    /// 保存节点位置
    /// </summary>
    public Vector3 _worldPos;
    /// <summary>
    /// 整个网格的索引
    /// </summary>
    public int _gridX, _gridY;

    public int gCost;
    public int hCost;
    //指针
    int heapIndex;

    public int fCost
    {
        get { return gCost + hCost; }
    }
    /// <summary>
    /// 内部构造指针
    /// </summary>
    public int HeapIndex
    {
        get
        {
            return heapIndex;
        }

        set
        {
            heapIndex = value;
        }
    }

    public Node parent;

    public Node(bool _canWalk, Vector3 _worldPos, int _gridX, int _gridY)
    {
        this._canWalk = _canWalk;
        this._worldPos = _worldPos;
        this._gridX = _gridX;
        this._gridY = _gridY;
    }
    /// <summary>
    /// 比较估值
    /// </summary>
    /// <param name="other"></param>
    /// <returns></returns>
    public int CompareTo(Node nodeToCompare)
    {
        //a.compareto(b) a<b=-1 a=b=0 a>b=1
        int compare = fCost.CompareTo(nodeToCompare.fCost);
        if (compare==0)
        {
            compare = hCost.CompareTo(nodeToCompare.hCost);
        }
        return compare;
    }
}

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class FindPath : MonoBehaviour
{
    public Transform player, EndPoint;
    Grid grid;
    // Use this for initialization
    void Start()
    {
        grid = GetComponent<Grid>();
    }

    // Update is called once per frame
    void Update()
    {
        FindingPath(player.position, EndPoint.position);
    }

    void FindingPath(Vector3 StarPos, Vector3 EndPos)
    {
        Node startNode = grid.GetFromPos(StarPos);
        Node endNode = grid.GetFromPos(EndPos);
        // List<Node> openSet = new List<Node>();
        //根据地图大小实例化堆的容量
        Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
        HashSet<Node> closeSet = new HashSet<Node>();
        openSet.Add(startNode);

        while (openSet.Count() > 0)
        {
            //Node currentNode = openSet[0];

            //for (int i = 0; i < openSet.Count; i++)
            //{
            //    if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)
            //    {
            //        currentNode = openSet[i];
            //    }
            //}

            Node currentNode = openSet.RemoveFirst();
            //openSet.Remove(currentNode);
            closeSet.Add(currentNode);

            if (currentNode == endNode)
            {
                GeneratePath(startNode,endNode);
                return;
            }
            //判断周围最优节点
            foreach (var item in grid.GetNeibourhood(currentNode))
            {
                if (!item._canWalk || closeSet.Contains(item))
                    continue;
                int newCost = currentNode.gCost + GetDistanceNodes(currentNode, item);
                if (newCost<item.gCost||!openSet.Contains(item))
                {
                    item.gCost = newCost;
                    item.hCost = GetDistanceNodes(item, endNode);
                    item.parent = currentNode;
                    if (!openSet.Contains(item))
                    {
                        openSet.Add(item);
                    }
                }
            }
        }
    }

    private void GeneratePath(Node startNode,Node endNode)
    {
        List<Node> path = new List<Node>();
        Node temp = endNode;
        while (temp!=startNode)
        {
            path.Add(temp);
            temp = temp.parent;
        }
        //列表反转
        path.Reverse();
        grid.path = path;
    }

    int GetDistanceNodes(Node a, Node b)
    {
        //估算权值,对角线算法 看在X轴还是Y轴格子数多  可计算斜移动
        int cntX = Mathf.Abs(a._gridX - b._gridX);
        int cntY = Mathf.Abs(a._gridY - b._gridY);
        if (cntX > cntY)
        {
            return 14 * cntY + 10 * (cntX - cntY);
        }
        else
        {
            return 14 * cntX + 10 * (cntY - cntX);
        }

        //曼哈顿算法
        //return Mathf.Abs(a._gridX - b._gridX) * 10 + Mathf.Abs(a._gridY - b._gridY) * 10;
    }
}

在两个目标点高速移动下少了一点
这个是源码连接
https://github.com/1004019267/AStar3D/tree/master