/// <summary>
/// Performs path search using A* algorithm for a given start and list of goals.
/// <para>Returns list of nodes from best ending node to starting node.</para>
/// <para>Returns null if there is no path to any of the goals.</para>
/// </summary>
/// <param name="start">Starting node</param>
/// <param name="goals">List of ending nodes</param>
/// <param name="limiter">Maximum length of path to search for</param>
public static List <MainNode> MultiAStar(MainNode start, List <T> goals, int limiter = int.MaxValue)
{
if (goals.All(g => g == null) || start == null)
{
return(null);
}
Stopwatch s = Stopwatch.StartNew();
var open = new Heap <MainNode> {
MinHeap = true
};
var closed = new HashSet <MainNode>();
var gs = new Dictionary <MainNode, float>();
var fs = new Dictionary <MainNode, float>();
var cameFrom = new Dictionary <MainNode, MainNode>();
open.Add(start, 0.0f);
while (open.Count > 0)
{
MainNode current = open.PopFirst().Object;
if (goals.Exists(t => current.NodeEqual(t)))
{
open.Clear();
closed.Clear();
var t = ToList(current, cameFrom);
s.Stop();
Add(ref _at, s.Elapsed.TotalMilliseconds);
return(t);
}
closed.Add(current);
if (gs.GetValueOrDefault(current, 0f) > limiter)
{
continue;
}
foreach (MainNode neighbor in current.GetNeighbors())
{
var num = gs.GetValueOrDefault(current, 0f) + current.Distance(neighbor);
if ((closed.Contains(neighbor) ||
open.Contains(neighbor)) && num >= gs.GetValueOrDefault(neighbor, 0f))
{
continue;
}
cameFrom[neighbor] = current;
gs[neighbor] = num;
fs[neighbor] = gs[neighbor] + goals.Min(t => neighbor.Heuristic(t));
if (!open.Contains(neighbor))
{
open.Add(neighbor, fs[neighbor]);
}
}
}
open.Clear();
closed.Clear();
s.Stop();
Add(ref _at, s.Elapsed.TotalMilliseconds);
return(null);
}
/// <summary>
/// Performs path search using Dijkstra's algorithm for a given predicate.
/// <para>Returns list of nodes from ending to starting node.</para>
/// <para>Returns null if there is no path to goal.</para>
/// </summary>
/// <param name="start">Starting node</param>
/// <param name="targeting">Predicate distinguishing end nodes from the rest</param>
/// <param name="limiter">Maximum length of path to search for</param>
public static List <MainNode> Dijkstra(MainNode start, Predicate <MainNode> targeting, int limiter = int.MaxValue)
{
if (start == null)
{
return(null);
}
Stopwatch s = Stopwatch.StartNew();
var open = new Heap <MainNode> {
MinHeap = true
};
var closed = new HashSet <MainNode>();
var gs = new Dictionary <MainNode, float>();
var cameFrom = new Dictionary <MainNode, MainNode>();
open.Add(start, 0.0f);
while (open.Count > 0)
{
MainNode current = open.PopFirst().Object;
if (targeting.Invoke(current))
{
open.Clear();
closed.Clear();
var t = ToList(current, cameFrom);
s.Stop();
Add(ref _dt, s.Elapsed.TotalMilliseconds);
return(t);
}
closed.Add(current);
if (gs.GetValueOrDefault(current, 0f) > limiter)
{
continue;
}
foreach (MainNode neighbor in current.GetNeighbors())
{
var num = gs.GetValueOrDefault(current, 0f) + current.Distance(neighbor);
if ((closed.Contains(neighbor) ||
open.Contains(neighbor)) && num >= gs.GetValueOrDefault(neighbor, 0f))
{
continue;
}
cameFrom[neighbor] = current;
gs[neighbor] = num;
if (!open.Contains(neighbor))
{
open.Add(neighbor, gs.GetValueOrDefault(neighbor, 0f));
}
}
}
open.Clear();
closed.Clear();
s.Stop();
Add(ref _dt, s.Elapsed.TotalMilliseconds);
return(null);
}
private static List <MainNode> ToList(MainNode current, IDictionary <MainNode, MainNode> cameFrom)
{
var objList = new List <MainNode>();
while (current != null)
{
objList.Add(current);
current = cameFrom.GetValueOrDefault(current, null);
}
return(objList);
}
/// <summary> /// Approximates distance between two nodes using given metric. /// </summary> /// <param name="goal">Second node to measure distance to</param> public abstract float Heuristic(MainNode goal);
/// <summary> /// Returns distance between two nodes using given metric. /// </summary> /// <param name="b">Second node to measure distance to</param> public abstract float Distance(MainNode b);
/// <summary> /// Checks if two nodes are equal. /// </summary> /// <param name="b">Second node to check</param> public abstract bool NodeEqual(MainNode b);
/// <summary>
/// Checks if two nodes are equal.
/// </summary>
/// <param name="b">Second node to check</param>
public override bool NodeEqual(MainNode b)
{
return(((HashNode)b).Id == Id);
}
public override float Heuristic(MainNode goal)
{
Manhattan2DNode manhattan2Dnode = (Manhattan2DNode)goal;
return(Math.Abs(manhattan2Dnode.X - X) + Math.Abs(manhattan2Dnode.Y - Y));
}
public override float Distance(MainNode b)
{
return((10 + ((Manhattan2DNode)b).Wall) * 0.1f);
}
public override bool NodeEqual(MainNode b)
{
Manhattan2DNode manhattan2Dnode = (Manhattan2DNode)b;
return(manhattan2Dnode.X == X && manhattan2Dnode.Y == Y);
}
