geometry-tower-defense-base/src-ref/Scene/Pathfinding/GridMapPathfinder.cs

using System.Collections.Generic;
using UnityEngine;

namespace GeometryTD.Pathfinding
{
    public class GridMapPathfinder : IMapPathfinder
    {
        private static readonly Vector3Int[] NeighborOffsets =
        {
            new Vector3Int(1, 0, 0),
            new Vector3Int(-1, 0, 0),
            new Vector3Int(0, 1, 0),
            new Vector3Int(0, -1, 0)
        };

        private readonly HashSet<Vector3Int> _walkableSet = new HashSet<Vector3Int>();
        private readonly Queue<Vector3Int> _openQueue = new Queue<Vector3Int>();
        private readonly Dictionary<Vector3Int, Vector3Int> _cameFrom = new Dictionary<Vector3Int, Vector3Int>();

        public bool TryFindPath(
            IReadOnlyCollection<Vector3Int> walkableCells,
            Vector3Int startCell,
            Vector3Int destinationCell,
            IReadOnlyCollection<Vector3Int> blockedCells,
            List<Vector3Int> pathResult)
        {
            if (pathResult == null)
            {
                return false;
            }

            pathResult.Clear();
            if (walkableCells == null || walkableCells.Count <= 0)
            {
                return false;
            }

            BuildWalkableSet(walkableCells);
            if (!_walkableSet.Contains(startCell) || !_walkableSet.Contains(destinationCell))
            {
                return false;
            }

            _openQueue.Clear();
            _cameFrom.Clear();
            _openQueue.Enqueue(startCell);
            _cameFrom[startCell] = startCell;

            while (_openQueue.Count > 0)
            {
                Vector3Int currentCell = _openQueue.Dequeue();
                if (currentCell == destinationCell)
                {
                    BuildPath(destinationCell, pathResult);
                    return true;
                }

                for (int i = 0; i < NeighborOffsets.Length; i++)
                {
                    Vector3Int nextCell = currentCell + NeighborOffsets[i];
                    if (!_walkableSet.Contains(nextCell) || _cameFrom.ContainsKey(nextCell))
                    {
                        continue;
                    }

                    if (IsBlocked(nextCell, blockedCells, startCell, destinationCell))
                    {
                        continue;
                    }

                    _cameFrom[nextCell] = currentCell;
                    _openQueue.Enqueue(nextCell);
                }
            }

            return false;
        }

        private void BuildWalkableSet(IReadOnlyCollection<Vector3Int> walkableCells)
        {
            _walkableSet.Clear();
            foreach (Vector3Int cell in walkableCells)
            {
                _walkableSet.Add(cell);
            }
        }

        private void BuildPath(Vector3Int destinationCell, List<Vector3Int> pathResult)
        {
            pathResult.Clear();
            Vector3Int current = destinationCell;
            while (true)
            {
                pathResult.Add(current);
                Vector3Int parent = _cameFrom[current];
                if (parent == current)
                {
                    break;
                }

                current = parent;
            }

            pathResult.Reverse();
        }

        private static bool IsBlocked(
            Vector3Int cell,
            IReadOnlyCollection<Vector3Int> blockedCells,
            Vector3Int startCell,
            Vector3Int destinationCell)
        {
            if (blockedCells == null || blockedCells.Count <= 0)
            {
                return false;
            }

            if (cell == startCell || cell == destinationCell)
            {
                return false;
            }

            foreach (Vector3Int blockedCell in blockedCells)
            {
                if (blockedCell == cell)
                {
                    return true;
                }
            }

            return false;
        }
    }
}