SquareGrid + path finding

PathFinding.cs

using System.Collections.Generic;
using Unity.Mathematics;

namespace Grids.Square
{
    public class PathFinding
    {
        private const int MOVE_STRAIGHT_COST = 10;
        private const int MOVE_DIAGONAL_COST = 14;
        
        private readonly SquareGrid<PathNode> _grid;
        private readonly List<PathNode> _openList = new(); //for searching
        private readonly List<PathNode> _closedList = new(); //already searched
        private readonly List<PathNode> _neighborsBuffer = new();

        public PathFinding(int width, int height)
        {
            _grid = new SquareGrid<PathNode>(width, height, 1f, float3.zero, (grid, x, y) => new PathNode(grid, x, y));
        }

        public void SetWalkable(bool walkable, int x, int y)
        {
            var cell = _grid.GetValue(x, y);
            if (cell != null)
            {
                cell.IsWalkable = walkable;
            }
        }

        public List<float3> FindPath(float3 start, float3 end)
        {
            var startIndexes = _grid.GetIndexes(start);
            var endIndexes = _grid.GetIndexes(end);
            var path = FindPath(startIndexes, endIndexes);
            if (path == null)
            {
                return null;
            }

            var vectorPath = new List<float3>(path.Count);
            for (var i = 0; i < path.Count; i++)
            {
                var p = path[i];
                vectorPath.Add(_grid.GetWorldPosition(p.X, p.Y));
            }

            return vectorPath;
        }
        
        public List<PathNode> FindPath(int2 start, int2 end)
        {
            _openList.Clear();
            _closedList.Clear();

            var startNode = _grid.GetValue(start.x, start.y);
            var endNode = _grid.GetValue(end.x, end.y);
            _openList.Add(startNode);

            //reset values
            for (int x = 0; x < _grid.Width; x++)
            {
                for (int y = 0; y < _grid.Height; y++)
                {
                    var node = _grid.GetValue(x, y);
                    node.GCost = int.MaxValue;
                    node.CalculateFCost();
                    node.CameFrom = null;
                }
            }

            startNode.GCost = 0;
            startNode.HCost = CalculateDistance(startNode, endNode);
            startNode.CalculateFCost();

            while (_openList.Count > 0)
            {
                var currentNode = GetLowestFCostNode(_openList);
                if (currentNode == endNode)
                {
                    return CalculatePath(endNode);
                }

                _openList.Remove(currentNode);
                _closedList.Add(currentNode);

                _neighborsBuffer.Clear();
                CollectNeighborsPath(currentNode, _neighborsBuffer);
                for (int i = 0; i < _neighborsBuffer.Count; i++)
                {
                    var neighborNode = _neighborsBuffer[i];
                    if (_closedList.Contains(neighborNode))
                    {
                        //already searched
                        continue;
                    }

                    if (!neighborNode.IsWalkable)
                    {
                        _closedList.Add(neighborNode);
                        continue;
                    }

                    var tentativeGCost = currentNode.GCost + CalculateDistance(currentNode, neighborNode);
                    if (tentativeGCost >= neighborNode.GCost)
                    {
                        continue;
                    }
                    
                    neighborNode.CameFrom = currentNode;
                    neighborNode.GCost = tentativeGCost;
                    neighborNode.HCost = CalculateDistance(neighborNode, endNode);
                    neighborNode.CalculateFCost();

                    if (!_openList.Contains(neighborNode))
                    {
                        _openList.Add(neighborNode);
                    }
                }
            }
            
            //failed to find the path
            return null;
        }

        private void CollectNeighborsPath(PathNode node, List<PathNode> neighborsBuffer)
        {
            for (var x = -1; x < 2; x++)
            {
                for (var y = -1; y < 2; y++)
                {
                    if (x == 0 && y == 0)
                    {
                        //skip original node
                        continue;
                    }
                    
                    CollectNeighborsPath(node.X + x, node.Y + y, neighborsBuffer);
                }
            }
        }

        private void CollectNeighborsPath(int x, int y, ICollection<PathNode> neighborsBuffer)
        {
            if (x < 0 || y < 0 || x >= _grid.Width || y >= _grid.Height)
            {
                return;
            }
            
            neighborsBuffer.Add(GetNode(x, y));
        }

        private PathNode GetNode(int x, int y)
        {
            return _grid.GetValue(x, y);
        }
        
        private List<PathNode> CalculatePath(PathNode endNode)
        {
            var path = new List<PathNode> {endNode};

            var currentNode = endNode;

            while (currentNode.CameFrom != null)
            {
                path.Add(currentNode.CameFrom);
                currentNode = currentNode.CameFrom;
            }
            
            path.Reverse();
            
            return path;
        }
        
        private int CalculateDistance(PathNode a, PathNode b)
        {
            var xDist = math.abs(a.X - b.X);
            var yDist = math.abs(a.Y - b.Y);
            var remaining = math.abs(xDist - yDist);
            return MOVE_DIAGONAL_COST * math.min(xDist, yDist) + MOVE_STRAIGHT_COST * remaining;
        }

        private PathNode GetLowestFCostNode(IReadOnlyList<PathNode> nodes)
        {
            var node = nodes[0];
            for (int i = 1; i < nodes.Count; i++)
            {
                if (nodes[i].FCost < node.FCost)
                {
                    node = nodes[i];
                }
            }

            return node;
        }
    }
}

SquareGrid.cs

using Unity.Mathematics;
using UnityEngine;
using UnityEngine.Events;

namespace Grids.Square
{
    public class SquareGrid<T>
    {
        private readonly UnityEvent<OnValueChangedArgs> _onValueChanged = new ();
        public class OnValueChangedArgs
        {
            public int X;
            public int Y;
        }

        private readonly int _width;
        private readonly int _height;
        private readonly T[] _cells;
        private readonly IIndexesHelper _indexesHelper;
        
        public int Width => _width;
        public int Height => _height;

        public SquareGrid(int width, int height, float cellSize, float3 originPosition, System.Func<SquareGrid<T>, int, int, T> createCell, bool xzPlane = true)
        {
            _width = width;
            _height = height;

            _cells = new T[width * height];

            _indexesHelper = xzPlane
                ? new IndexesHelperXZ(cellSize, originPosition)
                : new IndexesHelperXY(cellSize, originPosition);

            for (int x = 0; x < width; x++)
            {
                for (int y = 0; y < height; y++)
                {
                    var index = x + y * _width;
                    _cells[index] = createCell(this, x, y);
                }
            }
        }

        public float3 GetWorldPosition(int x, int y)
        {
            return _indexesHelper.GetWorldPosition(x, y);
        }

        public int2 GetIndexes(float3 worldPos)
        {
            return _indexesHelper.GetIndexes(worldPos);
        }

        public void SetValue(float3 worldPos, T value)
        {
            var indexes = GetIndexes(worldPos);
            SetValue(indexes.x, indexes.y, value);
        }
        public void SetValue(int x, int y, T value)
        {
            var index = x + y * _width;
            if (index < _cells.Length)
            {
                _cells[index] = value;
                TriggerOnValueChanged(x, y);
            }
            else
            {
                Debug.LogError($"Index out of range : {index} ({x}:{y})");
            }
        }

        public T GetValue(float3 worldPos)
        {
            var indexes = GetIndexes(worldPos);
            return GetValue(indexes.x, indexes.y);
        }
        public T GetValue(int x, int y)
        {
            var index = x + y * _width;
            return index < _cells.Length ? _cells[index] : default;
        }

        public void TriggerOnValueChanged(int x, int y)
        {
            _onValueChanged.Invoke(new OnValueChangedArgs() {X = x, Y = y});
        }

        public void SubscribeOnValueChanged(UnityAction<OnValueChangedArgs> action)
        {
            _onValueChanged.AddListener(action);
        }
        public void UnsubscribeOnValueChanged(UnityAction<OnValueChangedArgs> action)
        {
            _onValueChanged.RemoveListener(action);
        }

        public void DrawGridDebug()
        {
            var mapWidth = _width;
            var mapHeight = _height;

            var color = Color.white;
            for (int x = 0; x < mapWidth; x++)
            {
                for (int y = 0; y < mapHeight; y++)
                {
                    DrawCell(x, y, color);
                }
            }

            Debug.DrawLine(GetWorldPosition(0, mapHeight), GetWorldPosition(mapWidth, mapHeight), color, 100f);
            Debug.DrawLine(GetWorldPosition(mapWidth, 0), GetWorldPosition(mapWidth, mapHeight), color, 100f);
        }

        private void DrawCell(int x, int y, Color color)
        {
            Debug.DrawLine(GetWorldPosition(x, y), GetWorldPosition(x, y + 1), color, 100f);
            Debug.DrawLine(GetWorldPosition(x, y), GetWorldPosition(x + 1, y), color, 100f);
        }

        #region IndexesHelper

        private interface IIndexesHelper
        {
            float3 GetWorldPosition(int x, int y);
            int2 GetIndexes(float3 worldPos);
        }
        private class IndexesHelperXZ : IIndexesHelper
        {
            private readonly float _cellSize;
            private readonly float3 _originPosition;

            public IndexesHelperXZ(float cellSize, float3 originPosition)
            {
                _cellSize = cellSize;
                _originPosition = originPosition;
            }
            
            public float3 GetWorldPosition(int x, int y)
            {
                return new float3(x, 0, y) * _cellSize + _originPosition;
            }

            public int2 GetIndexes(float3 worldPos)
            {
                var pos = worldPos - _originPosition;
                return new int2((int) math.floor(pos.x / _cellSize),
                                (int) math.floor(pos.z / _cellSize));
            }
        }
        private class IndexesHelperXY : IIndexesHelper
        {
            private readonly float _cellSize;
            private readonly float3 _originPosition;

            public IndexesHelperXY(float cellSize, float3 originPosition)
            {
                _cellSize = cellSize;
                _originPosition = originPosition;
            }
            
            public float3 GetWorldPosition(int x, int y)
            {
                return new float3(x, y, 0) * _cellSize + _originPosition;
            }

            public int2 GetIndexes(float3 worldPos)
            {
                var pos = worldPos - _originPosition;
                return new int2((int) math.floor(pos.x / _cellSize),
                                (int) math.floor(pos.y / _cellSize));
            }
        }
        
        #endregion //IndexesHelper
    }
}