Random shortest path A*

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

class Pathfinding
{
    enum CellState
    {
        Unfound = 0,
        Discovered = 1
    }

    struct FrontierElement : IComparable<FrontierElement>
    {
        public HexXY coords;
        public float cost;
        public int youth; //we must expand oldest node first for path randomization, though it is slower

        public FrontierElement(HexXY coords, float cost, int youth)
        {
            this.coords = coords;
            this.cost = cost;
            this.youth = youth;
        }

        public int CompareTo(FrontierElement other)
        {
            int costComparison = cost.CompareTo(other.cost);
            if (costComparison != 0) return costComparison;
            else return youth.CompareTo(other.youth);
        }
    }

    System.Random rng;

    int size;
    CellState[] cellStates;
    float[] pathCosts;
    int[] moveDirs; //Every element of this is a bit mask of possible directions for shortest path randomization
    BinaryHeap<FrontierElement> frontier;

    public Pathfinding(int mapSize, int rngSeed)
    {
        this.size = mapSize;
        this.rng = new System.Random(rngSeed);
        this.cellStates = new CellState[size * size];
        this.moveDirs = new int[size * size];
        this.pathCosts = new float[size * size];
        this.frontier = new BinaryHeap<FrontierElement>(10);
    }

    void Clear()
    {
        Array.Clear(cellStates, 0, cellStates.Length);
        Array.Clear(moveDirs, 0, moveDirs.Length);
        Array.Clear(pathCosts, 0, pathCosts.Length);
        frontier.Reset();
    }

    void AddDir(int x, int y, int dir)
    {
        moveDirs[y * size + x] |= 1 << dir;
    }

    void GetPossibleDirs(int x, int y, List<int> dirs)
    {
        dirs.Clear();
        for (int i = 0; i < 6; i++)
            if ((moveDirs[y * size + x] & (1 << i)) != 0)
                dirs.Add(i);
    }

    void UnfoldPath(HexXY start, HexXY finish, List<HexXY> path, bool randomize)
    {
        path.Clear();
        if (start == finish)
            return;

        HexXY c = finish;
        List<int> possibleDirs = new List<int>(5);

        do
        {
            path.Add(c);
            GetPossibleDirs(c.x, c.y, possibleDirs);
            int dir;
            if (randomize)
                dir = possibleDirs[rng.Next(possibleDirs.Count)];
            else
                dir = possibleDirs[0];
            c -= HexXY.neighbours[dir];
        } while (c != start);
        path.Add(start);
        path.Reverse();
    }

    public static float StandardHeuristic(HexXY goal, HexXY c)
    {
        return HexXY.Dist(goal, c);
    }

    public static bool StandardGoal(HexXY goal, HexXY c, int dist = 1)
    {
        return HexXY.Dist(goal, c) <= dist;
    }

    bool IsInsideMap(HexXY c)
    {
        return c.x >= 0 && c.x < size && c.y >= 0 && c.y < size;
    }

    public bool FindPath(HexXY start,
                         Func<HexXY, float> funcDistToGoalHeuristic,
                         Func<HexXY, bool> predicateGoalReached,
                         Func<HexXY, float> funcCost,
                         float maxCost,
                         bool randomize,
                         List<HexXY> path)
    {
        Clear();

        frontier.Enqueue(new FrontierElement(start, funcDistToGoalHeuristic(start), 0));
        cellStates[start.y * size + start.x] = CellState.Discovered;

        int expandedNodes = 0;

        do
        {
            FrontierElement el = frontier.Dequeue();
            HexXY c = el.coords;
            //Debug.Log("Expanding " + c.ToString() + ", cost + heuristic " + el.cost.ToString());
            if (predicateGoalReached(c))
            {
                UnfoldPath(start, c, path, randomize);
                return true;
            }

            float currentCost = pathCosts[c.y * size + c.x];
            if (currentCost <= maxCost)
            {
                for (int i = 0; i < 6; i++)
                {
                    HexXY n = c + HexXY.neighbours[i];
                    if (!IsInsideMap(n))
                        continue;

                    float cost = currentCost + funcCost(n);
                    if (cellStates[n.y * size + n.x] == CellState.Discovered)
                    {
                        //If we can go to this discovered cell with the same cost,
                        //why not use this additional direction for path randomization?
                        //We'll get all shortest paths basically for free
                        //(all possible shortest paths in fact, if we consider that A* with admissable heuristic
                        //iteratively improves lower bound on path length)
                        //P.S. actually we need a particular order of node expanding (expand oldest first) for
                        //this to work

                        //DEBUG
                        //Debug.Log("Another dir ---------------");
                        //Debug.Log(n);
                        //Debug.Log(cost);
                        //Debug.Log(pathCosts[n.y * size + n.x]);

                        if (pathCosts[n.y * size + n.x] == cost)
                            AddDir(n.x, n.y, i);
                        continue;
                    }

                    float h = funcDistToGoalHeuristic(n);
                    float value = cost + h;

                    frontier.Enqueue(new FrontierElement(n, value, expandedNodes));
                    AddDir(n.x, n.y, i);
                    cellStates[n.y * size + n.x] = CellState.Discovered;
                    pathCosts[n.y * size + n.x] = cost;

                    //Debug.Log("Adding " + n.ToString() + ", cost " + cost);
                }
            }

            expandedNodes++;
        } while (frontier.Count > 0);

        return false;
    }
}