Node

gistfile1.txt

using System;
using NPOI.Util.Collections;

namespace AStar
{
    /// <summary>
    ///     A node.
    /// </summary>
    public abstract class Node : IComparable<Node>
    {
        /// <summary>
        ///     Cost from source. G
        /// </summary>
        public float CostFromSource;

        /// <summary>
        ///     Cost to end. H
        /// </summary>
        public float CostToEnd;

        /// <summary>
        ///     The connections.
        /// </summary>
        public HashSet<Node> Connections;

        /// <summary>
        ///     Gets total cost. F
        /// </summary>
        public float TotalCost => CostFromSource + CostToEnd;

        /// <summary>
        ///     The parent node.
        /// </summary>
        public virtual Node ParentNode { get; set; }

        /// <summary>
        ///     Compares the current instance with another object of the same type and returns an integer that indicates
        ///     whether the current instance precedes, follows, or occurs in the same position in the sort order as the other
        ///     object.
        /// </summary>
        /// <param name="other">An object to compare with this instance. </param>
        /// <returns>
        ///     A value that indicates the relative order of the objects being compared. The return value has these meanings:
        ///     Value Meaning Less than zero This instance precedes <paramref name="other" /> in the sort order.  Zero This
        ///     instance occurs in the same position in the sort order as <paramref name="other" />. Greater than zero This
        ///     instance follows <paramref name="other" /> in the sort order.
        /// </returns>
        public int CompareTo(Node other)
        {
            if (ReferenceEquals(this, other))
            {
                return 0;
            }

            if (ReferenceEquals(null, other))
            {
                return 1;
            }

            return TotalCost.CompareTo(other.TotalCost);
        }

        /// <summary>
        ///     Calculates the g.
        /// </summary>
        /// <param name="sourceNode">   Source node. </param>
        /// <returns>
        ///     True if there was a G change.
        /// </returns>
        public abstract bool CalculateG(Node sourceNode);
    }
}

PathFinder

using System.Collections.Generic;
using FS.SDK.Collections;

namespace AStar
{
    /// <summary>
    ///     A pathfinder.
    /// </summary>
    public class Pathfinder
    {
        /// <summary>
        ///     Searches for a path between the starting node and the end node.
        /// </summary>
        /// <param name="startNode">    The start node. </param>
        /// <param name="endNode">      The end node. </param>
        /// <param name="path">         [out] Path to take. </param>
        /// <returns>
        ///     True if a path is found.
        /// </returns>
        public bool FindPath(Node startNode, Node endNode, out List<Node> path)
        {
            var openNodes = new BinaryHeap<Node>(BinaryHeapOrder.SmallFirst);
            var closedNodes = new HashSet<Node>();
            var inOpenNodes = new HashSet<Node>();

            openNodes.Push(startNode);

            while (openNodes.TryPop(out var currentNode))
            {
                if (currentNode == endNode)
                {
                    BackTrack(endNode, startNode, out path);
                    return true;
                }

                inOpenNodes.Remove(currentNode);
                closedNodes.Add(currentNode);
                foreach (var connection in currentNode.Connections)
                {
                    if (closedNodes.Contains(connection))
                    {
                        continue;
                    }

                    if (inOpenNodes.Contains(connection))
                    {
                        var needToUpdate = connection.CalculateG(currentNode);
                        if (needToUpdate)
                        {
                            connection.ParentNode = currentNode;
                            openNodes.AddOrUpdate(connection);
                        }
                    }
                    else
                    {
                        connection.ParentNode = currentNode;
                        openNodes.Push(connection);
                        inOpenNodes.Add(connection);
                    }
                }
            }

            path = null;
            return false;
        }

        private void BackTrack(Node endNode, Node startNode, out List<Node> path)
        {
            path = new List<Node>();
            var currentNode = endNode;
            while (currentNode != startNode)
            {
                path.Add(currentNode);
                currentNode = currentNode.ParentNode;
            }

            path.Reverse();
        }
    }
}