C# Basic Algorithms And Data Structures

public static class AStar
{  
    public class Node //The Node will represent a cell in the grid.
    {
        public Node(int row, int col)
        {
            Parent = null;
            
            Row = row;
            Col = col;

            G_Cost = 0;
            H_Cost = 0;
            F_Cost = 0;
        }

        public Node Parent { get; set; }
        
        public int Row { get; set; }
        public int Col { get; set; }

        public int G_Cost { get; set; } //Distance from starting node.
        public int H_Cost { get; set; } //Distance from end node.
        public int F_Cost { get; set; } //F_Cost = G_Cost + H_Cost
    }


    /// <summary>
    ///    Finds the shortest path between two points on a grid using the A* algorithm.
    /// </summary>
    /// <param name="grid"></param>
    /// <param name="start"></param>
    /// <param name="finish"></param>
    /// <returns></returns>
    public static List<Node> FindPath(int[,] grid, Node start, Node finish)
    {
        //Make a HashSet to store visited nodes.
        HashSet<Node> visitedNodes = new HashSet<Node>(comparer: new NodeEqualityComparer());
        //The SortedSet will store the open nodes. It's similar to a HashSet but it maintains the order of the items.
        HashSet<Node> openNodesSet = new HashSet<Node>(comparer: new NodeEqualityComparer());
        //Add the start node to the open nodes set.
        openNodesSet.Add(start);

        //Iterate until the open nodes set is empty
        while (openNodesSet.Count > 0)
        {
            //Get the node with the lowest F value from the open set.
            Node current = GetLowestFCostNode(openNodesSet);

            //Check if the current node is the goal. If so reconstruct and return the path.
            if (current.Row == finish.Row && current.Col == finish.Col)
                return ReconstructPath(current);

            //Add the current node to the list of already visited nodes.
            visitedNodes.Add(current);
            openNodesSet.Remove(current);

            //Generate the neighbouring nodes of the current node from the grid.
            Node[] neighbors = GetNeighbors(grid, current);

            //Iterate through each neighbor.
            foreach (Node neighbor in neighbors)
            {
                if (neighbor == null)
                    continue;

                //Check if the neighbor has already visited ...
                if (visitedNodes.Contains(neighbor))
                    continue; // ... if so skip it.

                //Calculate the summed G_Cost so far.
                //Here each step has a cost of 1. It's however possible to assign different costs to different steps.
                //For example, you could imagine the cost as the Z axis and assign a cost of 1 to a flat area and a cost of 2 or more to a slope.
                int summedG_cost = current.G_Cost + 1;

                //If the neighbor is not in the open set, add it.
                if (!openNodesSet.Contains(neighbor))
                    openNodesSet.Add(neighbor);
                else if (summedG_cost >= neighbor.G_Cost) //If the new G_Cost is higher than the old one ...
                    continue; //... this is not a better path so skip this neighbor.

                //Update the neighbor.
                neighbor.Parent = current;
                neighbor.G_Cost = summedG_cost;
                neighbor.H_Cost = CalcDistance(neighbor, finish);
                neighbor.F_Cost = neighbor.G_Cost + neighbor.H_Cost;
            }
        }

        //No path found.
        return null;
    }

    //Get the node with the lowest F value from the open set.
    private static Node GetLowestFCostNode(HashSet<Node> openSet)
    {
        Node lowestNode = null;
        int lowestFCost = int.MaxValue;

        foreach (Node node in openSet)
        {
            if (node.F_Cost < lowestFCost)
            {
                lowestFCost = node.F_Cost;
                lowestNode = node;
            }
        }

        return lowestNode;
    }

    //Get the neighbors of a node from the grid.
    private static Node[] GetNeighbors(int[,] grid, Node node)
    {
        Node[] neighbors = new Node[4];

        //Get the dimensions(height/width) of the grid.
        int numRows = grid.GetLength(0);
        int numCols = grid.GetLength(1);

        //Define the 4 possible directions within the arrays.
        //Then access them during the iteration. This way we can reduce the amount of code.
        int[] xDir = { -1, 0, 1, 0 };
        int[] yDir = { 0, -1, 0, 1 };

        //Iterate through the 4 possible directions.
        for (int i = 0; i < 4; i++)
        {
            //Calculate the new position by adding/subtracting 1 from the current node position.
            int newRow = node.Row + xDir[i];
            int newCol = node.Col + yDir[i];

            //Check if the new position is within the grid and not blocked, else skip this neighbor.
            bool isXWithinGrid = newCol >= 0 && newCol < numCols; //min. and max. check for col/x-axis.
            if(!isXWithinGrid)
                continue;

            bool isYWithinGrid = newRow >= 0 && newRow < numRows; //min. and max. check for row/y-axis.
            if (!isYWithinGrid)
                continue;
            
            bool blocked = grid[newRow, newCol] == 1; //Check if blocked.
            if (blocked)
                continue;

            //If the neighbor is valid, create a new node and add it to the neighbors array.
            neighbors[i] = new Node(newRow, newCol);
        }

        return neighbors;
    }

    //Reconstruct the path from the finish node to the start node.
    private static List<Node> ReconstructPath(Node currentNode)
    {
        List<Node> path = new List<Node>();
        while (currentNode != null) //Iterate until we reach the node without a parent(start node).
        {
            //Save the current node to the path list ...
            path.Add(currentNode);
            //... and set the current node to its parent.
            currentNode = currentNode.Parent;
        }

        //Reverse the path to get the correct order.
        path.Reverse();

        return path;
    }

    //Calculate the shortest path between two points/nodes.
    private static int CalcDistance(Node current, Node finish)
    {
        //Manhattan distance(straight line) is used to calculate the distance between two points on a grid.
        return Math.Abs(current.Row - finish.Row) + Math.Abs(current.Col - finish.Col);
    }

    //Custom comparer for nodes.
    class NodeComparer : IComparer<Node>
    {
        public int Compare(Node x, Node y)
        {
            if (x.F_Cost == y.F_Cost)
                return 0;

            if (x.F_Cost < y.F_Cost)
                return -1;

            return 1;
        }
    }

    class NodeEqualityComparer : IEqualityComparer<Node>
    {
        public bool Equals(Node x, Node y)
        {
            return x.Row == y.Row && x.Col == y.Col;
        }

        public int GetHashCode(Node obj)
        {
            return obj.Row.GetHashCode() ^ obj.Col.GetHashCode();
        }
    }
}