public static SearchResult BestFirstSearch(Grid grid, Point startPos, Point endPos)
        {
            FakePriorityQueue <Point> queue       = new FakePriorityQueue <Point>();
            Dictionary <Point, float> distanceMap = new Dictionary <Point, float>();
            Dictionary <Point, Point> visitedMap  = new Dictionary <Point, Point>();

            queue.Enqueue(startPos, 0);
            distanceMap.Add(startPos, 0);
            visitedMap.Add(startPos, null);

            while (!queue.Empty)
            {
                Point current = queue.Dequeue();
                if (current.Equals(endPos))
                {
                    return(new SearchResult
                    {
                        Path = GeneratePath(visitedMap, current),
                        Visited = new List <Point>(visitedMap.Keys)
                    });
                }

                foreach (Point neighbour in grid.GetAdjacentCells(current))
                {
                    if (!visitedMap.ContainsKey(neighbour))
                    {
                        float priority = Heuristic(endPos, neighbour);
                        queue.Enqueue(neighbour, priority);
                        visitedMap.Add(neighbour, current);
                        distanceMap.Add(neighbour, distanceMap[current] + grid.GetCostOfEnteringCell(neighbour));
                    }
                }
            }
            return(new SearchResult());
        }
        public static SearchResult DijkstraGeneral(Grid grid, Point startPos)
        {
            FakePriorityQueue <Point> queue       = new FakePriorityQueue <Point>();
            Dictionary <Point, float> distanceMap = new Dictionary <Point, float>();
            Dictionary <Point, Point> visitedMap  = new Dictionary <Point, Point>();

            queue.Enqueue(startPos, 0);

            distanceMap.Add(startPos, 0);
            visitedMap.Add(startPos, null);

            while (!queue.Empty)
            {
                Point current = queue.Dequeue();

                foreach (Point adj in grid.GetAdjacentCells(current))
                {
                    float newDist = distanceMap[current] + grid.GetCostOfEnteringCell(adj);
                    if (!distanceMap.ContainsKey(adj) || newDist < distanceMap[adj])
                    {
                        distanceMap[adj] = newDist;
                        visitedMap[adj]  = current;
                        queue.Enqueue(adj, newDist);
                    }
                }
            }
            return(new SearchResult()
            {
                VisitedMap = visitedMap,
                DistanceMap = distanceMap
            });
        }
        public static SearchResult AStarSearchWithCost(Grid grid, Point startPos, Point endPos,
                                                       Dictionary <Point, float> costMap)
        {
            FakePriorityQueue <Point> queue     = new FakePriorityQueue <Point>();
            Dictionary <Point, float> costSoFar = new Dictionary <Point, float>();
            Dictionary <Point, Point> cameFrom  = new Dictionary <Point, Point>();

            queue.Enqueue(startPos, 0);
            costSoFar[startPos] = 0;
            cameFrom[startPos]  = null;

            while (!queue.Empty)
            {
                Point current = queue.Dequeue();
                if (current.Equals(endPos))
                {
                    return(new SearchResult
                    {
                        Path = GeneratePath(cameFrom, current),
                        Visited = new List <Point>(cameFrom.Keys)
                    });
                }
                foreach (Point neighbour in grid.GetAdjacentCells(current))
                {
                    float cost    = costMap.ContainsKey(neighbour) ? costMap[neighbour] : 1;
                    float newCost = costSoFar[current] + cost;
                    if (!costSoFar.ContainsKey(neighbour) || newCost < costSoFar[neighbour])
                    {
                        costSoFar[neighbour] = newCost;

                        float priority = newCost + Heuristic(endPos, neighbour);
                        queue.Enqueue(neighbour, priority);

                        cameFrom[neighbour] = current;
                    }
                }
            }
            return(new SearchResult());
        }