Frame/Assets/Scripts/BFS/BFS.cs

using System;
using System.Collections.Generic;
using Cysharp.Threading.Tasks;
using JetBrains.Annotations;
using UnityEngine;

namespace Game.Pathfinding;

public class Node<T>
{
    public int index { get; private set; }
    public T data { get; private set; }

    public Node(int index, T data)
    {
        this.index = index;
        this.data = data;
    }
}

class Edge<T>
{
    public Node<T> from { get; private set; }
    public Node<T> to { get; private set; }
    public bool canTranslate { get; private set; }

    public Edge(Node<T> from, Node<T> to, bool canTranslate = true)
    {
        this.from = from;
        this.to = to;
        this.canTranslate = canTranslate;
    }

    public Node<T> GetAnotherNode(Node<T> node)
    {
        return node == this.from ? to : to == node ? from : throw new InvalidOperationException();
    }
}

public class WayPointGraph : Graph<WayPoint>
{
    readonly struct UniqueWayPoint : IEquatable<UniqueWayPoint>
    {
        private readonly WayPoint _wayPoint;

        public UniqueWayPoint(WayPoint wayPoint)
        {
            this._wayPoint = wayPoint;
        }

        public bool Equals(UniqueWayPoint other)
        {
            return _wayPoint.position == other._wayPoint.position;
        }

        public override bool Equals(object obj)
        {
            return obj is UniqueWayPoint other && Equals(other);
        }

        public override int GetHashCode()
        {
            return _wayPoint.position.GetHashCode();
        }

        public static implicit operator WayPoint(UniqueWayPoint uniqueWayPoint)
        {
            return uniqueWayPoint._wayPoint;
        }

        public static implicit operator UniqueWayPoint(WayPoint wayPoint)
        {
            return new UniqueWayPoint(wayPoint);
        }
    }

    public void Initialize(GameGlobalConfig.NodeMap[] nodeMaps)
    {
        HashSet<UniqueWayPoint> wayPoints = new HashSet<UniqueWayPoint>();
        foreach (var nodeMap in nodeMaps)
        {
            wayPoints.Add(new WayPoint(nodeMap.node1));
            wayPoints.Add(new WayPoint(nodeMap.node2));
        }

        Dictionary<Vector3, Node<WayPoint>> position2NodeMap = new Dictionary<Vector3, Node<WayPoint>>();
        _nodes = new Node<WayPoint>[wayPoints.Count];
        using var enumerator = wayPoints.GetEnumerator();
        int index = 0;
        while (enumerator.MoveNext())
        {
            var enumeratorCurrent = (WayPoint)enumerator.Current;
            var node = new Node<WayPoint>(index, enumeratorCurrent);
            this._nodes[index] = node;
            position2NodeMap[enumeratorCurrent.position] = node;
            index++;
        }

        _edges = new List<Edge<WayPoint>>(nodeMaps.Length);
        this._connection = new Dictionary<Node<WayPoint>, List<Edge<WayPoint>>>(_edges.Count);

        foreach (var nodeMap in nodeMaps)
        {
            if (position2NodeMap.TryGetValue(nodeMap.node1, out var node1))
            {
                if (position2NodeMap.TryGetValue(nodeMap.node2, out var node2))
                {
                    var edge = new Edge<WayPoint>(node1, node2);
                    _edges.Add(edge);
                    if (!this._connection.TryGetValue(node1, out var list1))
                    {
                        this._connection[node1] = list1 = new List<Edge<WayPoint>>();
                    }

                    list1.Add(edge);
                    if (!this._connection.TryGetValue(node2, out var list2))
                    {
                        this._connection[node2] = list2 = new List<Edge<WayPoint>>();
                    }

                    list2.Add(edge);
                }
            }
        }
    }
}

public class Graph<T>
{
    private protected Node<T>[] _nodes;
    private protected List<Edge<T>> _edges;
    private protected Dictionary<Node<T>, List<Edge<T>>> _connection;

    internal Node<T> GetNode(int index)
    {
        if (index < 0 || index >= this._nodes.Length)
        {
            throw new IndexOutOfRangeException($"Index is {index}");
        }

        return this._nodes[index];
    }

    [CanBeNull]
    internal Node<T> GetNode(Predicate<Node<T>> predicate)
    {
        foreach (var node in this._nodes)
        {
            if (predicate(node))
            {
                return node;
            }
        }

        return null;
    }

    internal IReadOnlyList<Edge<T>> GetNeighbours(Node<T> node)
    {
        if (this._connection.TryGetValue(node, out var edges))
        {
            return edges;
        }

        throw new ArgumentException($"GetNeighbours have error ! the id is {node.index}");
    }
}

public class BFS<T>
{
    private Graph<T> _graph;
    private List<int> _closeList = new List<int>();
    private Queue<int> _openList = new Queue<int>();
    private Dictionary<Node<T>, Node<T>> _nodeMap = new Dictionary<Node<T>, Node<T>>();
    private List<int> ids = new List<int>();

    public BFS(Graph<T> graph)
    {
        this._graph = graph;
    }

    [CanBeNull]
    public Node<T> GetNode(Predicate<Node<T>> predicate)
    {
        return _graph.GetNode(predicate);
    }

    public Path<T> FindPath(Node<T> begin, Node<T> end)
    {
        if (begin == null)
            throw new NullReferenceException("begin");
        if (end == null)
            throw new NullReferenceException("end");
        _openList.Clear();
        _closeList.Clear();
        _nodeMap.Clear();

        _openList.Enqueue(begin.index);
        while (this._openList.Count > 0)
        {
            var nodeIndex = this._openList.Dequeue();
            var currNode = this._graph.GetNode(nodeIndex);
            if (currNode.index == end.index)
                break;
            _closeList.Add(currNode.index);
            Search(currNode);
        }

        Path<T> path = new Path<T>();
        path.AddNode(new PathNode<T>(end.index, end.data));
        this.ids.Clear();

        bool isTrue = true;
        UniTask.Create(async () =>
        {
            await UniTask.Delay(5000);
            if (isTrue)
            {
                isTrue = false;
                Debug.LogError("没找到路径卡死了，强制跳出的");
            }
        }).Forget();
        Node<T> node = end;
        while (isTrue)
        {
            if (this._nodeMap.TryGetValue(node, out node))
            {

                if (!this.ids.Contains(node.index))
                {
                    Debug.Log($"正在查询的点{node.index}");
                    this.ids.Add(node.index);
                    path.AddNode(new PathNode<T>(node.index, node.data));
                }
            }
            
            if (node==null || node == begin)
            {
                Debug.Log("结束了");
                break;
            }
        }

        path.Reverse();
        return path;
    }

    private void Search(Node<T> node)
    {
        var readOnlyList = this._graph.GetNeighbours(node);
        foreach (var edge in readOnlyList)
        {
            if (!edge.canTranslate)
                continue;
            Node<T> another = edge.GetAnotherNode(node);
            if (!this._nodeMap.ContainsKey(another))
            {
                _nodeMap[another] = node;
            }

            if (!this._closeList.Contains(another.index))
            {
                if (!this._openList.Contains(another.index))
                {
                    _openList.Enqueue(another.index);
                }
            }
        }
    }
}

public class Path<T>
{
    private readonly List<PathNode<T>> _pathNodes = new List<PathNode<T>>();

    public void Reverse()
    {
        this._pathNodes.Reverse();
    }

    public void AddNode(PathNode<T> node)
    {
        if (this._pathNodes.Contains(node))
        {
            throw new ArgumentException($"{node.index}");
        }

        this._pathNodes.Add(node);
    }

    public IReadOnlyList<PathNode<T>> GetNodes()
    {
        return this._pathNodes;
    }

    public void GetDatas(IList<T> result)
    {
        foreach (var pathNode in this._pathNodes)
        {
            result.Add(pathNode.data);
        }
    }
}

public class PathNode<T>
{
    public int index { get; private set; }
    public T data { get; private set; }

    public PathNode(int index, T data)
    {
        this.index = index;
        this.data = data;
    }
}