示例#1
0
        /// <summary>
        /// Calculate a path from the start location to the destination location
        /// </summary>
        /// <remarks>
        /// Based on the A* pathfinding algorithm described on Wikipedia
        /// </remarks>
        /// <see href="https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode"/>
        /// <param name="start">Start location</param>
        /// <param name="goal">Destination location</param>
        /// <param name="allowUnsafe">Allow possible but unsafe locations</param>
        /// <returns>A list of locations, or null if calculation failed</returns>
        public static Queue<Location> CalculatePath(World world, Location start, Location goal, bool allowUnsafe = false)
        {
            Queue<Location> result = null;

            AutoTimeout.Perform(() =>
            {
                HashSet<Location> ClosedSet = new HashSet<Location>(); // The set of locations already evaluated.
                HashSet<Location> OpenSet = new HashSet<Location>(new[] { start });  // The set of tentative nodes to be evaluated, initially containing the start node
                Dictionary<Location, Location> Came_From = new Dictionary<Location, Location>(); // The map of navigated nodes.

                Dictionary<Location, int> g_score = new Dictionary<Location, int>(); //:= map with default value of Infinity
                g_score[start] = 0; // Cost from start along best known path.
                // Estimated total cost from start to goal through y.
                Dictionary<Location, int> f_score = new Dictionary<Location, int>(); //:= map with default value of Infinity
                f_score[start] = (int)start.DistanceSquared(goal); //heuristic_cost_estimate(start, goal)

                while (OpenSet.Count > 0)
                {
                    Location current = //the node in OpenSet having the lowest f_score[] value
                        OpenSet.Select(location => f_score.ContainsKey(location)
                        ? new KeyValuePair<Location, int>(location, f_score[location])
                        : new KeyValuePair<Location, int>(location, int.MaxValue))
                        .OrderBy(pair => pair.Value).First().Key;
                    if (current == goal)
                    { //reconstruct_path(Came_From, goal)
                        List<Location> total_path = new List<Location>(new[] { current });
                        while (Came_From.ContainsKey(current))
                        {
                            current = Came_From[current];
                            total_path.Add(current);
                        }
                        total_path.Reverse();
                        result = new Queue<Location>(total_path);
                    }
                    OpenSet.Remove(current);
                    ClosedSet.Add(current);
                    foreach (Location neighbor in GetAvailableMoves(world, current, allowUnsafe))
                    {
                        if (ClosedSet.Contains(neighbor))
                            continue;		// Ignore the neighbor which is already evaluated.
                        int tentative_g_score = g_score[current] + (int)current.DistanceSquared(neighbor); //dist_between(current,neighbor) // length of this path.
                        if (!OpenSet.Contains(neighbor))	// Discover a new node
                            OpenSet.Add(neighbor);
                        else if (tentative_g_score >= g_score[neighbor])
                            continue;		// This is not a better path.

                        // This path is the best until now. Record it!
                        Came_From[neighbor] = current;
                        g_score[neighbor] = tentative_g_score;
                        f_score[neighbor] = g_score[neighbor] + (int)neighbor.DistanceSquared(goal); //heuristic_cost_estimate(neighbor, goal)
                    }
                }
            }, TimeSpan.FromSeconds(5));

            return result;
        }
示例#2
0
        /// <summary>
        /// Calculate a path from the start location to the destination location
        /// </summary>
        /// <remarks>
        /// Based on the A* pathfinding algorithm described on Wikipedia
        /// </remarks>
        /// <see href="https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode"/>
        /// <param name="start">Start location</param>
        /// <param name="goal">Destination location</param>
        /// <param name="allowUnsafe">Allow possible but unsafe locations</param>
        /// <returns>A list of locations, or null if calculation failed</returns>
        public static Queue <Location> CalculatePath(World world, Location start, Location goal, bool allowUnsafe = false)
        {
            Queue <Location> result = null;

            AutoTimeout.Perform(() =>
            {
                HashSet <Location> ClosedSet = new HashSet <Location>();                           // The set of locations already evaluated.
                HashSet <Location> OpenSet   = new HashSet <Location>(new[] { start });            // The set of tentative nodes to be evaluated, initially containing the start node
                Dictionary <Location, Location> Came_From = new Dictionary <Location, Location>(); // The map of navigated nodes.

                Dictionary <Location, int> g_score = new Dictionary <Location, int>();             //:= map with default value of Infinity
                g_score[start] = 0;                                                                // Cost from start along best known path.
                // Estimated total cost from start to goal through y.
                Dictionary <Location, int> f_score = new Dictionary <Location, int>();             //:= map with default value of Infinity
                f_score[start] = (int)start.DistanceSquared(goal);                                 //heuristic_cost_estimate(start, goal)

                while (OpenSet.Count > 0)
                {
                    Location current = //the node in OpenSet having the lowest f_score[] value
                                       OpenSet.Select(location => f_score.ContainsKey(location)
                        ? new KeyValuePair <Location, int>(location, f_score[location])
                        : new KeyValuePair <Location, int>(location, int.MaxValue))
                                       .OrderBy(pair => pair.Value).First().Key;
                    if (current == goal)
                    { //reconstruct_path(Came_From, goal)
                        List <Location> total_path = new List <Location>(new[] { current });
                        while (Came_From.ContainsKey(current))
                        {
                            current = Came_From[current];
                            total_path.Add(current);
                        }
                        total_path.Reverse();
                        result = new Queue <Location>(total_path);
                    }
                    OpenSet.Remove(current);
                    ClosedSet.Add(current);
                    foreach (Location neighbor in GetAvailableMoves(world, current, allowUnsafe))
                    {
                        if (ClosedSet.Contains(neighbor))
                        {
                            continue;                                                                      // Ignore the neighbor which is already evaluated.
                        }
                        int tentative_g_score = g_score[current] + (int)current.DistanceSquared(neighbor); //dist_between(current,neighbor) // length of this path.
                        if (!OpenSet.Contains(neighbor))                                                   // Discover a new node
                        {
                            OpenSet.Add(neighbor);
                        }
                        else if (tentative_g_score >= g_score[neighbor])
                        {
                            continue;           // This is not a better path.
                        }
                        // This path is the best until now. Record it!
                        Came_From[neighbor] = current;
                        g_score[neighbor]   = tentative_g_score;
                        f_score[neighbor]   = g_score[neighbor] + (int)neighbor.DistanceSquared(goal); //heuristic_cost_estimate(neighbor, goal)
                    }
                }
            }, TimeSpan.FromSeconds(5));

            return(result);
        }
        /// <summary>
        /// Calculate a path from the start location to the destination location
        /// </summary>
        /// <remarks>
        /// Based on the A* pathfinding algorithm described on Wikipedia
        /// </remarks>
        /// <see href="https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode"/>
        /// <param name="start">Start location</param>
        /// <param name="goal">Destination location</param>
        /// <param name="allowUnsafe">Allow possible but unsafe locations</param>
        /// <param name="maxOffset">If no valid path can be found, also allow locations within specified distance of destination</param>
        /// <param name="minOffset">Do not get closer of destination than specified distance</param>
        /// <param name="ct">Token for stopping computation after a certain time</param>
        /// <returns>A list of locations, or null if calculation failed</returns>
        public static Queue <Location> CalculatePath(World world, Location start, Location goal, bool allowUnsafe, int maxOffset, int minOffset, CancellationToken ct)
        {
            // This is a bad configuration
            if (minOffset > maxOffset)
            {
                throw new ArgumentException("minOffset must be lower or equal to maxOffset", "minOffset");
            }

            // Round start coordinates for easier calculation
            start = new Location(Math.Floor(start.X), Math.Floor(start.Y), Math.Floor(start.Z));

            // We always use distance squared so our limits must also be squared.
            minOffset *= minOffset;
            maxOffset *= maxOffset;

            ///---///
            // Prepare variables and datastructures for A*
            ///---///

            // Dictionary that contains the relation between all coordinates and resolves the final path
            Dictionary <Location, Location> CameFrom = new Dictionary <Location, Location>();
            // Create a Binary Heap for all open positions => Allows fast access to Nodes with lowest scores
            BinaryHeap openSet = new BinaryHeap();
            // Dictionary to keep track of the G-Score of every location
            Dictionary <Location, int> gScoreDict = new Dictionary <Location, int>();

            // Set start values for variables
            openSet.Insert(0, (int)start.DistanceSquared(goal), start);
            gScoreDict[start] = 0;
            BinaryHeap.Node current = null;

            ///---///
            // Start of A*
            ///---///

            // Execute while we have nodes to process and we are not cancelled
            while (openSet.Count() > 0 && !ct.IsCancellationRequested)
            {
                // Get the root node of the Binary Heap
                // Node with the lowest F-Score or lowest H-Score on tie
                current = openSet.GetRootLocation();

                // Return if goal found and no maxOffset was given OR current node is between minOffset and maxOffset
                if ((current.Location == goal && maxOffset <= 0) || (maxOffset > 0 && current.H_score >= minOffset && current.H_score <= maxOffset))
                {
                    return(ReconstructPath(CameFrom, current.Location));
                }

                // Discover neighbored blocks
                foreach (Location neighbor in GetAvailableMoves(world, current.Location, allowUnsafe))
                {
                    // If we are cancelled: break
                    if (ct.IsCancellationRequested)
                    {
                        break;
                    }

                    // tentative_gScore is the distance from start to the neighbor through current
                    int tentativeGScore = current.G_score + (int)current.Location.DistanceSquared(neighbor);

                    // If the neighbor is not in the gScoreDict OR its current tentativeGScore is lower than the previously saved one:
                    if (!gScoreDict.ContainsKey(neighbor) || (gScoreDict.ContainsKey(neighbor) && tentativeGScore < gScoreDict[neighbor]))
                    {
                        // Save the new relation between the neighbored block and the current one
                        CameFrom[neighbor]   = current.Location;
                        gScoreDict[neighbor] = tentativeGScore;

                        // If this location is not already included in the Binary Heap: save it
                        if (!openSet.ContainsLocation(neighbor))
                        {
                            openSet.Insert(tentativeGScore, (int)neighbor.DistanceSquared(goal), neighbor);
                        }
                    }
                }
            }

            //// Goal could not be reached. Set the path to the closest location if close enough
            if (current != null && (maxOffset == int.MaxValue || openSet.MinH_ScoreNode.H_score <= maxOffset))
            {
                return(ReconstructPath(CameFrom, openSet.MinH_ScoreNode.Location));
            }
            else
            {
                return(null);
            }
        }