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 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());
    }
Beispiel #3
0
    public static SearchResult DijkstraPriority(Grid grid, Grid.Point startPos, Grid.Point endPos)
    {
        FakePriorityQueue <Grid.Point>      queue       = new FakePriorityQueue <Grid.Point>();
        Dictionary <Grid.Point, float>      distanceMap = new Dictionary <Grid.Point, float>();
        Dictionary <Grid.Point, Grid.Point> visitedMap  = new Dictionary <Grid.Point, Grid.Point>();

        queue.Enqueue(startPos, 0);

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

        while (!queue.Empty)
        {
            Grid.Point current = queue.Dequeue();
            if (current.Equals(endPos))
            {
                return(new SearchResult
                {
                    Path = GeneratePath(visitedMap, current),
                    Visited = new List <Grid.Point>(visitedMap.Keys)
                });
            }
            foreach (Grid.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());
    }
    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());
    }
Beispiel #5
0
        public static SearchResult AStarSearch(Grid grid, Grid.Point startPos, Grid.Point endPos)
        {
            FakePriorityQueue <Grid.Point>      queue       = new FakePriorityQueue <Grid.Point>();
            Dictionary <Grid.Point, float>      distanceMap = new Dictionary <Grid.Point, float>();
            Dictionary <Grid.Point, Grid.Point> visitedMap  = new Dictionary <Grid.Point, Grid.Point>();

            queue.Enqueue(startPos, 0);
            distanceMap[startPos] = 0;
            visitedMap[startPos]  = null;

            while (!queue.Empty)
            {
                Grid.Point current = queue.Dequeue();
                if (current.Equals(endPos))
                {
                    return(new SearchResult
                    {
                        Path = GeneratePath(visitedMap, current),
                        Visited = new List <Grid.Point>(visitedMap.Keys)
                    });
                }
                foreach (Grid.Point neighbour in grid.GetAdjacentCells(current))
                {
                    float newCost = distanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
                    if (!distanceMap.ContainsKey(neighbour) || newCost < distanceMap[neighbour])
                    {
                        distanceMap[neighbour] = newCost;

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

                        visitedMap[neighbour] = current;
                    }
                }
            }
            return(new SearchResult());
        }
        public static SearchResult BiDirectionalAStarSearch(Grid grid, Grid.Point startPos, Grid.Point endPos)
        {
            // True if opened from the start queue, false if opened by the end queue, not opened if not exists
            Dictionary <Grid.Point, bool> openedBy = new Dictionary <Grid.Point, bool>();

            FakePriorityQueue <Grid.Point> startQueue = new FakePriorityQueue <Grid.Point>();
            FakePriorityQueue <Grid.Point> endQueue   = new FakePriorityQueue <Grid.Point>();

            Dictionary <Grid.Point, float>      startDistanceMap = new Dictionary <Grid.Point, float>();
            Dictionary <Grid.Point, Grid.Point> startVisitedMap  = new Dictionary <Grid.Point, Grid.Point>();
            Dictionary <Grid.Point, float>      endDistanceMap   = new Dictionary <Grid.Point, float>();
            Dictionary <Grid.Point, Grid.Point> endVisitedMap    = new Dictionary <Grid.Point, Grid.Point>();

            startQueue.Enqueue(startPos, 0);
            startDistanceMap[startPos] = 0;
            startVisitedMap[startPos]  = null;
            openedBy[startPos]         = true;

            endQueue.Enqueue(endPos, 0);
            endDistanceMap[endPos] = 0;
            endVisitedMap[endPos]  = null;
            openedBy[endPos]       = false;

            while (!startQueue.Empty && !endQueue.Empty)
            {
                // From start
                Grid.Point current = startQueue.Dequeue();
                if (openedBy.ContainsKey(current) && openedBy[current] == false)
                {
                    // Found goal or the frontier from the end queue
                    // Return solution
                    List <Grid.Point> startPath = GeneratePath(startVisitedMap, current);
                    List <Grid.Point> endPath   = GeneratePath(endVisitedMap, current);

                    List <Grid.Point> allPath = new List <Grid.Point>(startPath);
                    allPath.AddRange(endPath);

                    List <Grid.Point> allVisited = new List <Grid.Point>(startVisitedMap.Keys);
                    allVisited.AddRange(endVisitedMap.Keys);

                    return(new SearchResult
                    {
                        Path = allPath,
                        Visited = allVisited
                    });
                }
                foreach (Grid.Point neighbour in grid.GetAdjacentCells(current))
                {
                    float newCost = startDistanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
                    if (!startDistanceMap.ContainsKey(neighbour) || newCost < startDistanceMap[neighbour])
                    {
                        startDistanceMap[neighbour] = newCost;
                        openedBy[neighbour]         = true;

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

                        startVisitedMap[neighbour] = current;
                    }
                }
                // From end
                current = endQueue.Dequeue();
                if (openedBy.ContainsKey(current) && openedBy[current] == true)
                {
                    // Found goal or the frontier from the start queue
                    // Return solution
                    List <Grid.Point> startPath = GeneratePath(startVisitedMap, current);
                    List <Grid.Point> endPath   = GeneratePath(endVisitedMap, current);

                    List <Grid.Point> allPath = new List <Grid.Point>(startPath);
                    allPath.AddRange(endPath);

                    List <Grid.Point> allVisited = new List <Grid.Point>(startVisitedMap.Keys);
                    allVisited.AddRange(endVisitedMap.Keys);

                    return(new SearchResult
                    {
                        Path = allPath,
                        Visited = allVisited
                    });
                }
                foreach (Grid.Point neighbour in grid.GetAdjacentCells(current))
                {
                    float newCost = endDistanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
                    if (!endDistanceMap.ContainsKey(neighbour) || newCost < endDistanceMap[neighbour])
                    {
                        endDistanceMap[neighbour] = newCost;
                        openedBy[neighbour]       = false;

                        float priority = newCost + Heuristic(startPos, neighbour);
                        endQueue.Enqueue(neighbour, priority);

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