A Star C# Unity

AStar.cs

using System;
using System.Collections.Generic;
using UnityEngine;

// Similar to https://lsreg.ru/realizaciya-algoritma-poiska-a-na-c/

/*

List<Vector2Int> path = AStar.FindPath(
    current, target, 200, cell => { return isCleanToMovement(cell); }
);

*/

public class AStar
{

    public static List<Vector2Int> FindPath(
        Vector2Int start,
        Vector2Int goal,
        int calculationLimit,
        CanMove canMove
    )
    {
        var closedSet = new PathNodeCollection();
        var openSet = new PathNodeCollection();

        PathNode startNode = new PathNode()
        {
            Position = start,
            CameFrom = null,
            PathLengthFromStart = 0,
            HeuristicEstimatePathLength = GetHeuristicPathLength(start, goal)
        };
        openSet.Add(startNode);
        int currIteration = 0;

        while (openSet.Count() > 0)
        {
            currIteration++;
            if (currIteration >= calculationLimit)
            {
                return null;
            }

            var currentNode = openSet.FindShortestFullPathLength();

            if (currentNode.Position == goal)
            {
                return GetPathForNode(currentNode);
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            foreach (var neighbourNode in Get4Neighbours(currentNode, goal, canMove))
            {
                if (closedSet.HasPosition(neighbourNode.Position))
                {
                    continue;
                }
                var openNode = openSet.FindForPosition(neighbourNode.Position);

                if (openNode == null)
                {
                    openSet.Add(neighbourNode);
                }
                else
                    if (openNode.PathLengthFromStart > neighbourNode.PathLengthFromStart)
                {
                    openNode.CameFrom = currentNode;
                    //TODO Optimise somehow: because of variative PathLengthFromStart 
                    //can't just store sorted by EstimateFullPathLength()
                    openNode.PathLengthFromStart = neighbourNode.PathLengthFromStart;
                }
            }
        }
        return null;
    }

    private static List<PathNode> Get4Neighbours(PathNode pathNode, Vector2Int goal, CanMove canMove)
    {
        var result = new List<PathNode>(4);

        Vector2Int[] neighbourPoints = new Vector2Int[4];
        neighbourPoints[0] = new Vector2Int(pathNode.Position.x + 1, pathNode.Position.y);
        neighbourPoints[1] = new Vector2Int(pathNode.Position.x - 1, pathNode.Position.y);
        neighbourPoints[2] = new Vector2Int(pathNode.Position.x, pathNode.Position.y + 1);
        neighbourPoints[3] = new Vector2Int(pathNode.Position.x, pathNode.Position.y - 1);

        foreach (Vector2Int point in neighbourPoints)
        {
            if (!canMove(point))
            {
                continue;
            }

            var neighbourNode = new PathNode()
            {
                Position = point,
                CameFrom = pathNode,
                PathLengthFromStart = pathNode.PathLengthFromStart + GetDistanceBetweenNeighbours(),
                HeuristicEstimatePathLength = GetHeuristicPathLength(point, goal)
            };
            result.Add(neighbourNode);
        }
        return result;
    }

    private static int GetDistanceBetweenNeighbours()
    {
        return 1;
    }

    private static int GetHeuristicPathLength(Vector2Int from, Vector2Int to)
    {
        return Math.Abs(from.x - to.x) + Math.Abs(from.y - to.y);
    }

    private static List<Vector2Int> GetPathForNode(PathNode pathNode)
    {
        var result = new List<Vector2Int>();
        var currentNode = pathNode;
        while (currentNode != null)
        {
            result.Add(currentNode.Position);
            currentNode = currentNode.CameFrom;
        }
        result.Reverse();
        return result;
    }

    public delegate bool CanMove(Vector2Int cell);
}

class PathNodeCollection
{

    private readonly List<PathNode> Nodes = new List<PathNode>();
    private readonly Dictionary<Vector2Int, PathNode> PositionToNode =
        new Dictionary<Vector2Int, PathNode>();

    public int Count() => Nodes.Count;

    public void Add(PathNode element)
    {
        Nodes.Add(element);
        PositionToNode[element.Position] = element;
    }

    public void Remove(PathNode element)
    {
        Nodes.Remove(element);
        PositionToNode.Remove(element.Position);
    }

    public PathNode FindShortestFullPathLength()
    {
        if (Nodes.Count == 0)
        {
            return null;
        }
        if (Nodes.Count == 1)
        {
            return Nodes[0];
        }
        int shortest = Nodes[0].EstimateFullPathLength();
        int shortestIndex = 0;
        for (int i = 1; i < Nodes.Count; i++)
        {
            int len = Nodes[i].EstimateFullPathLength();
            if (len < shortest)
            {
                shortestIndex = i;
            }
        }
        return Nodes[shortestIndex];
    }

    public bool HasPosition(Vector2Int pos)
    {
        return PositionToNode.ContainsKey(pos);
    }

    public PathNode FindForPosition(Vector2Int pos)
    {
        PathNode result = null;
        PositionToNode.TryGetValue(pos, out result);
        return result;
    }
}

class PathNode
{

    public Vector2Int Position;

    public int PathLengthFromStart;

    public PathNode CameFrom;

    public int HeuristicEstimatePathLength;

    public int EstimateFullPathLength()
    {
        return this.PathLengthFromStart + this.HeuristicEstimatePathLength;
    }
}