Untitled

    public class Pathfinding
    {
        public static Pathfinding ArenaPathfinding;

        // Called at start to create an instance for the arena's pathfinding
        public static void CreateArenaPathfinding(Tilemap arenaTilemap)
        {
            ArenaPathfinding = new Pathfinding(arenaTilemap);
        }

        // Diagonal cost is roughly sqrt(2) times more expensive, mathematically
        private const int MoveStraightCost = 10;
        private const int MoveDiagonalCost = 14;

        private PathNode[,] _grid;
        private Tilemap _tilemap;

        private int tilemapWidth;
        private int tilemapHeight;

        // We use a hashset due to it having the quickest lookup time.
        private HashSet<PathNode> _closedNodes = new();
        
        public Pathfinding(Tilemap obstacleTilemap)
        {
            _tilemap = obstacleTilemap;

            // Cut off edges with null tiles to save compute
            _tilemap.CompressBounds();

            tilemapWidth = _tilemap.cellBounds.size.x;
            tilemapHeight = _tilemap.cellBounds.size.y;

            _grid = new PathNode[tilemapWidth, tilemapHeight];

            // Loop through tilemap to convert to PathNode objects
            for (var x = 0; x < tilemapWidth; x++)
            {
                for (var y = 0; y < tilemapHeight; y++)
                {
                    _grid[x, y] = new PathNode(_grid, x, y);
                    
                    if (_tilemap.GetTile(new Vector3Int(x, y) + _tilemap.cellBounds.min) != null)
                    {
                        _closedNodes.Add(_grid[x, y]);
                    }
                }
            }
        }

        public List<Vector2> GetPath(Vector2 startPos, Vector2 endPos)
        {

            // Convert input coordinates into tile coordinates.
            var startTile = _tilemap.WorldToCell(startPos) - _tilemap.cellBounds.min;
            var endTile = _tilemap.WorldToCell(endPos) - _tilemap.cellBounds.min; // - _tilemap.cellBounds.min;

            var startNode = _grid[Mathf.Clamp(startTile.x, 0, _grid.GetLength(0)), Mathf.Clamp(startTile.y, 0, _grid.GetLength(1))];
            var endNode = _grid[Mathf.Clamp(endTile.x, 0, _grid.GetLength(0)), Mathf.Clamp(endTile.y, 0, _grid.GetLength(1))];

            var openList = new HashSet<PathNode>() { startNode };//(new ByFCost());
            var closedList = new HashSet<PathNode>();

            foreach (var node in _closedNodes)
            {
                closedList.Add(node);
            }

            for (var x = 0; x < tilemapWidth; x++)
            {
                for (var y = 0; y < tilemapHeight; y++)
                {
                    var pathNode = _grid[x, y];
                    pathNode.gCost = int.MaxValue;
                    pathNode.CalculateFCost();
                    pathNode.cameFromNode = null;

                    // My code
                    if (_closedNodes.Contains(pathNode))
                    {
                        if (endNode == pathNode)
                        {
                            endNode = GetNearestNeighbor(endNode, endPos);
                        }

                        if (startNode == pathNode)
                        {
                            startNode = GetNearestNeighbor(startNode, startPos);
                        }
                    }
                    // End my code
                }
            }
            
            startNode.gCost = 0;
            startNode.hCost = CalculateDistanceCost(startNode, endNode);
            startNode.CalculateFCost();

            while (openList.Count > 0)
            {
                var currentNode = LowestFCostNode(openList);
                if (currentNode == endNode)
                {
                    return CalculatePath(endNode).Select(pn =>
                        (Vector2) _tilemap.CellToWorld(new Vector3Int(pn.X, pn.Y) + _tilemap.cellBounds.min) +
                        new Vector2(0.5f, 0.5f)).ToList();
                }

                openList.Remove(currentNode);
                
                closedList.Add(currentNode);

                foreach (var neighborNode in GetNeighborList(currentNode))
                {
                    if (closedList.Contains(neighborNode)) continue;

                    var tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighborNode);
                    if (tentativeGCost < neighborNode.gCost)
                    {
                        neighborNode.cameFromNode = currentNode;
                        neighborNode.gCost = tentativeGCost;
                        neighborNode.hCost = CalculateDistanceCost(neighborNode, endNode);
                        neighborNode.CalculateFCost();

                        openList.Add(neighborNode);
                    }
                }
            }

            // Out of nodes on the open list
            return new List<Vector2>(); // No path
        }

        private PathNode GetNearestNeighbor(PathNode current, Vector2 worldPos)
        {
            return GetNeighborList(current)
                .Where(pn => _tilemap.GetTile(new Vector3Int(pn.X, pn.Y) + _tilemap.cellBounds.min) == null)
                .OrderBy(pn => Vector2.Distance(worldPos - new Vector2(0f, 0.3f),
                    (Vector2) _tilemap.CellToWorld(new Vector3Int(pn.X, pn.Y) +
                                                   _tilemap.cellBounds.min) + new Vector2(0.5f, 0.5f)))
                .FirstOrDefault();
        }

        private HashSet<PathNode> GetNeighborList(PathNode currentNode)
        {
            var neighborList = new HashSet<PathNode>();

            if (currentNode.X - 1 >= 0)
            {
                neighborList.Add(_grid[currentNode.X - 1, currentNode.Y]); // Left
                if (currentNode.Y - 1 >= 0) neighborList.Add(_grid[currentNode.X - 1, currentNode.Y - 1]); // Left down
                if (currentNode.Y + 1 < _grid.GetLength(1))
                    neighborList.Add(_grid[currentNode.X - 1, currentNode.Y + 1]); // Left up
            }

            if (currentNode.X + 1 < _grid.GetLength(0))
            {
                neighborList.Add(_grid[currentNode.X + 1, currentNode.Y]); // Right
                if (currentNode.Y - 1 >= 0) neighborList.Add(_grid[currentNode.X + 1, currentNode.Y - 1]); // Right down
                if (currentNode.Y + 1 < _grid.GetLength(1))
                    neighborList.Add(_grid[currentNode.X + 1, currentNode.Y + 1]); // Right up
            }

            if (currentNode.Y - 1 >= 0) neighborList.Add(_grid[currentNode.X, currentNode.Y - 1]); // Down
            if (currentNode.Y + 1 < _grid.GetLength(1)) neighborList.Add(_grid[currentNode.X, currentNode.Y + 1]); // Up

            return neighborList;
        }

        private List<PathNode> CalculatePath(PathNode endNode)
        {
            var path = new List<PathNode> {endNode};
            var currentNode = endNode;

            while (currentNode.cameFromNode != null)
            {
                path.Add(currentNode.cameFromNode);
                currentNode = currentNode.cameFromNode;
            }

            path.Reverse();

            return path;
        }

        private int CalculateDistanceCost(PathNode a, PathNode b)
        {
            var xDistance = Mathf.Abs(a.X - b.X);
            var yDistance = Mathf.Abs(a.Y - b.Y);
            var remaining = Mathf.Abs(xDistance - yDistance);

            return MoveDiagonalCost * Mathf.Min(xDistance, yDistance) + MoveStraightCost * remaining;
        }

        private PathNode LowestFCostNode(HashSet<PathNode> pathNodeList)
        {
            return pathNodeList.OrderBy(pn => pn.fCost).FirstOrDefault();
        }
    }

    public class ByFCost : IComparer<PathNode>
    {
        public int Compare(PathNode x, PathNode y)
        {
            return x.fCost == y.fCost ? 0 : x.fCost > y.fCost ? 1 : -1;
        }
    }

    public class PathNode
    {
        private PathNode[,] _grid;

        public PathNode(PathNode[,] grid, int x, int y)
        {
            _grid = grid;
            X = x;
            Y = y;
        }

        public int X;
        public int Y;

        public int gCost;
        public int hCost;
        public int fCost;

        public void CalculateFCost()
        {
            fCost = gCost + hCost;
        }

        public PathNode cameFromNode;
    }
}