Pathfinding stuff

Graph.cs

    public abstract class Graph
    {
        public GraphNode[] Nodes;
        public abstract void GenerateGraph();
        public abstract float HeuristicCostEstimate(GraphNode current, GraphNode target);
        public bool Dirty;
    }

    // interface for graph nodes
    public abstract class GraphNode : IHeapItem<GraphNode>
    {
        // pathfinding data
        public GraphNode Parent { get; set; }
        public GraphEdge[] Edges { get; set; }
        public float gScore;
        public float hScore;
        public float fScore { get { return hScore + gScore; } }
        public float x;
        public float y;
        public float z;

        // IHeapItem implementation
        public int HeapIndex { get; set; }
        public int CompareTo(GraphNode other)
        {
            int result = fScore.CompareTo(other.fScore);
            if (result == 0)
            {
                result = hScore.CompareTo(other.hScore);
            }
            return -result;
        }
    }

    // struct for graph edges
    public struct GraphEdge
    {
        public GraphNode StartNode;
        public GraphNode EndNode;
        public float Cost;

        public GraphEdge(GraphNode startNode, GraphNode endNode, float cost)
        {
            StartNode = startNode;
            EndNode = endNode;
            Cost = cost;
        }
    }

Heap.cs

    // Data structure for Heap optimisation
    public class Heap<T> where T : IHeapItem<T>
    {
        // private member variables
        private T[] _items;
        private int _count;

        // public property for count (same as List<T>.Count)
        public int Count
        {
            get { return _count; }
        }

        // constructor
        public Heap(int maxHeapSize)
        {
            _items = new T[maxHeapSize];
        }

        // method to determine if heap contains an item
        public bool Contains(T item)
        {
            return Equals(_items[item.HeapIndex], item);
        }

        // method to add item to the heap
        public void Add(T item)
        {
            item.HeapIndex = _count;
            _items[_count] = item;
            SortUp(item);
            _count++;
        }

        // method to remove the first item of the heap
        public T RemoveFirst()
        {
            T firstItem = _items[0];
            _count--;
            _items[0] = _items[_count];
            _items[0].HeapIndex = 0;
            SortDown(_items[0]);
            return firstItem;
        }

        // method to update the heap indices
        public void UpdateItem(T item)
        {
            SortUp(item);
        }

        // method for moving an item down the heap
        void SortDown(T item)
        {
            while (true)
            {
                int childIndexLeft = item.HeapIndex * 2 + 1;
                int childIndexRight = item.HeapIndex * 2 + 2;

                if (childIndexLeft < _count)
                {
                    int swapIndex = childIndexLeft;

                    if (childIndexRight < _count)
                    {
                        if (_items[childIndexLeft].CompareTo(_items[childIndexRight]) < 0)
                        {
                            swapIndex = childIndexRight;
                        }
                    }

                    if (item.CompareTo(_items[swapIndex]) < 0)
                    {
                        Swap(item, _items[swapIndex]);
                    }
                    else
                    {
                        return;
                    }

                }
                else
                {
                    return;
                }
            }
        }

        // method for moving an item up the heap
        void SortUp(T item)
        {
            int parentIndex = (item.HeapIndex - 1) / 2;

            while (true)
            {
                T parentItem = _items[parentIndex];
                if (item.CompareTo(parentItem) > 0)
                {
                    Swap(item, parentItem);
                }
                else
                {
                    break;
                }
                parentIndex = (item.HeapIndex - 1) / 2;
            }
        }

        // method for swapping two items in the heap
        void Swap(T itemA, T itemB)
        {
            _items[itemA.HeapIndex] = itemB;
            _items[itemB.HeapIndex] = itemA;
            int itemAIndex = itemA.HeapIndex;
            itemA.HeapIndex = itemB.HeapIndex;
            itemB.HeapIndex = itemAIndex;
        }
    }

    // the interface items must implement to be added to the heap
    public interface IHeapItem<T> : IComparable<T>
    {
        int HeapIndex { get; set; }
    }

Pathfinding.cs

    public static class Pathfinding
    {
        // AStar Guided Search
        public static GraphNode[] FindPathAStar(Graph graph, GraphNode startNode, GraphNode targetNode, out bool success)
        { 
            // if the passed graph is dirty, update it
            if (graph.Dirty) graph.GenerateGraph();
 
            // initialise the return array
            GraphNode[] path = new GraphNode[0];
            success = false;

            // create an empty hash table for the closed set
            HashSet<GraphNode> closedSet = new HashSet<GraphNode>();

            // create a new heap to store the open set and add the start node
            Heap<GraphNode> openSet = new Heap<GraphNode>(graph.Nodes.Length);
            openSet.Add(startNode);

            // iterate through the open set
            while (openSet.Count > 0)
            {
                // grab the first node from the open set heap
                GraphNode currentNode = openSet.RemoveFirst();

                // for each of the members of the open set
                for (int i = 0; i <= openSet.Count && !success; i++)
                {
                    // move one to the closed set
                    closedSet.Add(currentNode);

                    // if this one is the target, we have suceeded
                    if (currentNode == targetNode)
                    {
                        success = true;
                        continue; // skip rest of iteration
                    }

                    // for each of this node's neighbors...
                    foreach (GraphEdge edge in currentNode.Edges)
                    {
                        GraphNode neighborNode = edge.EndNode;
                        if (closedSet.Contains(neighborNode)) continue;
                        float tempGScore = currentNode.gScore + graph.HeuristicCostEstimate(currentNode, neighborNode) + edge.Cost;

                        // if it's not a better path, try the next neighbor
                        if (!(tempGScore < neighborNode.gScore) && openSet.Contains(neighborNode)) continue;

                        // if it's a better path, update the scores for this node
                        neighborNode.gScore = tempGScore;
                        neighborNode.hScore = graph.HeuristicCostEstimate(neighborNode, targetNode);
                        neighborNode.Parent = currentNode;

                        // if it's not in the open set, add it, else update its position in the heap
                        if (!openSet.Contains(neighborNode)) openSet.Add(neighborNode); else openSet.UpdateItem(neighborNode);
                    }
                }
            }

            // if the target was reached, reconstruct the path
            if (success)
            {
                // create a new list to store the reconstructed path
                List<GraphNode> waypoints = new List<GraphNode>();
                GraphNode currentNode = targetNode;

                // starting from the target node, find each node's 'parent' until we reach the start
                while (currentNode != startNode)
                {
                    waypoints.Add(currentNode);
                    currentNode = currentNode.Parent;
                }

                // add the start node
                waypoints.Add(startNode);

                // convert to array and reverse
                path = waypoints.ToArray();
                Array.Reverse(path);
            }

            // if pathfinding failed, return null, otherwise return the reconstructed path
            return path;
        }

    }

TileGraph.cs

    /// <summary>
    /// Pathfinding Graph for use with the 2D tile map.
    /// Contains an array of TileGraphNodes.
    /// </summary>
    public class TileGraph : Graph
    {

        // private backing variable for the nodes
        private List<TileGraphNode> _nodes;
        private readonly Dictionary<Tile, GraphNode> _tileNodeMap = new Dictionary<Tile, GraphNode>();
        private readonly TileMap _map;

        // construct the graph
        public TileGraph(TileMap map)
        {
            _map = map;
            GenerateGraph();
            _map.TileDataChanged += (t) => Dirty = true;
        }

        // get a node from a map poisition
        public GraphNode GetNode(Tile tile)
        {
            GraphNode node;
            return _tileNodeMap.TryGetValue(tile, out node) ? node : null;
        }

        // generates the pathfinding graph
        public sealed override void GenerateGraph()
        {
            // first loop creates nodes for each navigable tile
            _nodes = new List<TileGraphNode>();

            // loop through the tiles in the map
            for (int y = 0; y < _map.SizeY; y++)
            {
                for (int x = 0; x < _map.SizeX; x++)
                {
                    // for each of the tiles
                    Tile t = _map.GetTile(x, y);
                    if (t == null || !t.Navigable) continue;

                    // create new node and add/update the dictionary
                    TileGraphNode newNode = new TileGraphNode(t);
                    _nodes.Add(newNode);
                    _tileNodeMap[t] = newNode;
                }
            }

            // second loop assigns edges using neighboring tiles
            foreach (TileGraphNode node in _nodes)
            {
                // initialise a list of edges
                List<GraphEdge> edges = new List<GraphEdge>();

                // get the neighboring nodes' tiles
                Tile t = node.Tile;
                Tile[] neighbors = t.GetNeighbors(true);

                // get cardinal neighbors navigable status
                bool n = neighbors[0] != null && neighbors[0].Navigable;
                bool e = neighbors[1] != null && neighbors[1].Navigable;
                bool s = neighbors[2] != null && neighbors[2].Navigable;
                bool w = neighbors[3] != null && neighbors[3].Navigable;

                // cardinals kill diagonals
                bool ne = (n && e) && neighbors[4] != null && neighbors[4].Navigable;
                bool se = (s && e) && neighbors[5] != null && neighbors[5].Navigable;
                bool sw = (s && w) && neighbors[6] != null && neighbors[6].Navigable;
                bool nw = (n && w) && neighbors[7] != null && neighbors[7].Navigable;

                // add these to a list if they are valid
                List<Tile> validNeighbors = new List<Tile>();
                if (n)  validNeighbors.Add(neighbors[0]);
                if (e)  validNeighbors.Add(neighbors[1]);
                if (s)  validNeighbors.Add(neighbors[2]);
                if (w)  validNeighbors.Add(neighbors[3]);
                if (ne) validNeighbors.Add(neighbors[4]);
                if (se) validNeighbors.Add(neighbors[5]);
                if (sw) validNeighbors.Add(neighbors[6]);
                if (nw) validNeighbors.Add(neighbors[7]);

                // assign edges for each of the valiud neighbors
                foreach (Tile neighbor in validNeighbors)
                {
                    edges.Add(new GraphEdge(node, GetNode(neighbor), neighbor.MovementCost));
                }

                // copy to node's 'edges' array
                node.Edges = edges.ToArray();
            }

            // from the private 2D array of TileGraphNodes, fill the 1D a
rray with GraphNodes
            List<GraphNode> nodeList = new List<GraphNode>();
            foreach (TileGraphNode node in _nodes)
            {
                if (node == null) continue;
                nodeList.Add(node);
            }
            Nodes = nodeList.ToArray();

            // reset the dirty flag
            Dirty = false;

            Debug.Log("Graph Generated with " + Nodes.Length + " Nodes.");
        }

        // heuristic cost estimate function using for pathfinding 
        public override float HeuristicCostEstimate(GraphNode a, GraphNode b)
        {
            float x = Math.Abs(a.x - b.x);
            float y = Math.Abs(a.y - b.y);

            if (x > y)
                return 2f * y + (x - y);
            return 2f * x + (y - x);
        }
    }

TileGraphNode.cs

    public class TileGraphNode : GraphNode
    {
        // store ref to the tile for convenience
        public Tile Tile;

        // ctor
        public TileGraphNode(Tile tile)
        {
            Tile = tile;
            x = Tile.PosX;
            y = Tile.PosY;
        }
    }