Frame/Assets/Scripts/BFS/BFS.cs

using System;
using System.Collections.Generic;
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 - 1)
            {
                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)
        {
            this._connection.TryGetValue(node, out var edges);
            return edges;
        }
    }

    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>>();

        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)
        {
            _openList.Clear();
            _closeList.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>();
            Node<T> curr = end;
            path.AddNode(new PathNode<T>(curr.index, curr.data));
            while (_nodeMap.TryGetValue(curr, out curr))
            {
                path.AddNode(new PathNode<T>(curr.index, curr.data));
                if (curr == begin)
                    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))
                {
                    _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)
        {
            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;
        }
    }