public TwoDPathFinderFast(ExpandedPathfindingGrid grid)
        {
            if (grid == null)
            {
                throw new Exception("Grid cannot be null");
            }

            _grid        = grid;
            mGridX       = (ushort)(_grid.GetWidth() + 1);
            mGridY       = (ushort)(_grid.GetHeight() + 1);
            mGridXMinus1 = (ushort)(mGridX - 1);
            mGridYLog2   = (ushort)Math.Log(mGridY, 2);

            // This should be done at the constructor, for now we leave it here.
            if ((Math.Log(mGridX, 2) != (int)Math.Log(mGridX, 2)) ||
                (Math.Log(mGridY, 2) != (int)Math.Log(mGridY, 2)))
            {
                throw new Exception("Invalid Grid, size in X and Y must be power of 2");
            }

            if ((mCalcGrid == null) || (mCalcGrid.Length != mGridX * mGridY))
            {
                mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];
            }

            _openNodes = new PriorityQueueB <int>(new ComparePFNodeMatrix(mCalcGrid));
        }
 public TwoDPathFinder(ExpandedPathfindingGrid grid)
 {
     if (grid == null)
     {
         throw new Exception("Grid cannot be null");
     }
     _grid = grid;
 }