/// <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>
/// <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);
}
}
