写一个Unity B星寻路算法

B星寻路算法（B* Pathfinding Algorithm）是对A*寻路算法的一种改进，它可以在运行中重新规划路径，以适应场景中动态的障碍物。下面是一个基于Unity的B星寻路算法的示例代码：

using System.Collections.Generic;
using UnityEngine;

public class BStarPathfinding : MonoBehaviour
{
    private Grid grid;
    private List<Node> openSet;
    private List<Node> closedSet;

    private void Awake()
    {
        grid = GetComponent<Grid>();
    }

    public List<Node> FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);

        openSet = new List<Node> { startNode };
        closedSet = new List<Node>();

        startNode.gCost = 0;
        startNode.hCost = Heuristic(startNode, targetNode);

        while (openSet.Count > 0)
        {
            Node currentNode = GetLowestFCostNode(openSet);
            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            if (currentNode == targetNode)
            {
                return RetracePath(startNode, targetNode);
            }

            foreach (Node neighbor in grid.GetNeighbors(currentNode))
            {
                if (closedSet.Contains(neighbor))
                {
                    continue;
                }

                float newMovementCostToNeighbor = currentNode.gCost + Heuristic(currentNode, neighbor);
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                    neighbor.gCost = newMovementCostToNeighbor;
                    neighbor.hCost = Heuristic(neighbor, targetNode);
                    neighbor.parent = currentNode;

                    if (!openSet.Contains(neighbor))
                    {
                        openSet.Add(neighbor);
                    }
                }
            }
        }

        return null;
    }

    private List<Node> RetracePath(Node startNode, Node endNode)
    {
        List<Node> path = new List<Node>();
        Node currentNode = endNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }

        path.Reverse();
        return path;
    }

    private Node GetLowestFCostNode(List<Node> nodeList)
    {
        Node lowestNode = nodeList[0];
        for (int i = 1; i < nodeList.Count; i++)
        {
            if (nodeList[i].fCost < lowestNode.fCost)
            {
                lowestNode = nodeList[i];
            }
        }
        return lowestNode;
    }

    private float Heuristic(Node nodeA, Node nodeB)
    {
        return Vector3.Distance(nodeA.worldPosition, nodeB.worldPosition);
    }
}

在这个示例代码中，我们首先定义了一个BStarPathfinding类，它包含了一个FindPath函数用于寻找路径。在FindPath函数中，我们首先获取起始节点和目标节点，并将起始节点加入到openSet中。接下来，我们进入一个while循环，直到openSet为空。在每一次循环中，我们获取fCost最小的节点，并将其从openSet中移除，并将其加入到closedSet中。如果当前节点是目标节点，我们就可以通过RetracePath函数得到路径。否则，我们遍历当前节点的所有邻居节点，并更新它们的gCost和hCost值。如果邻居节点不在openSet中，我们就将其加入到openSet中。

RetracePath函数用于从目标节点到起始节点遍历所有的父节点，并将它们加入到一个列表中。由于我们是从目标节点开始遍历的，所以最后需要将路径列表进行翻转。

GetLowestFCostNode函数用于获取fCost最小的节点，Heuristic函数用于计算两个节点之间的估价函数值。在这个示例代码中，我们使用了两个辅助函数来获取节点的邻居列表和将世界坐标转换为节点坐标的功能。这两个函数的具体实现可以参考A*寻路算法的示例代码。