public ConnectivityGraph(ConnectivityGraph <T> sub1, ConnectivityGraph <T> sub2)
{
foreach (var item in sub1.Union(sub2))
{
Add(item);
}
}
/// <summary>
/// Computes the subgraph of nodes that are accessible from the starting location or can access it.
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public ConnectivityGraph <T> ComputeSubgraph(T node)
{
var subgraph = new ConnectivityGraph <T>();
ComputeSubgraphRecursive(node, subgraph);
return(subgraph);
}
/// <summary>
/// Computes the subgraph of nodes that can access the starting location.
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public ConnectivityGraph <T> ComputeAccessTo(T end)
{
var subgraph = new ConnectivityGraph <T>();
ComputeAccessToRecursive(end, subgraph);
return(subgraph);
}
/// <summary>
/// Computes the subgraph of nodes that are accessible from the starting location.
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public ConnectivityGraph <T> ComputeAccessFrom(T start)
{
var subgraph = new ConnectivityGraph <T>();
ComputeAccessFromRecursive(start, subgraph);
return(subgraph);
}
private void ComputeAccessToRecursive(T end, ConnectivityGraph <T> subgraph)
{
foreach (var entrance in GetEntrances(end).Where(entrance => !subgraph.Contains(entrance)))
{
subgraph.Add(entrance);
subgraph.Connect(entrance, end);
ComputeAccessFromRecursive(entrance, subgraph);
}
}
private void ComputeAccessFromRecursive(T start, ConnectivityGraph <T> subgraph)
{
foreach (var exit in GetExits(start).Where(exit => !subgraph.Contains(exit)))
{
subgraph.Add(exit);
subgraph.Connect(start, exit);
ComputeAccessFromRecursive(exit, subgraph);
}
}
private void ComputeSubgraphRecursive(T node, ConnectivityGraph <T> subgraph)
{
foreach (var exit in GetExits(node).Where(exit => !subgraph.Contains(exit)))
{
subgraph.Add(exit);
subgraph.Connect(node, exit);
ComputeAccessFromRecursive(exit, subgraph);
}
foreach (var entrance in GetEntrances(node).Where(entrance => !subgraph.Contains(entrance)))
{
subgraph.Add(entrance);
subgraph.Connect(entrance, node);
ComputeAccessFromRecursive(entrance, subgraph);
}
}
/// <summary>
/// Navigation on an arbitrary connectivity graph.
/// TODO - optimize this algorithm just like the sector algorithm
/// </summary>
/// <param name="me"></param>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="avoidEnemies"></param>
/// <returns></returns>
public static IEnumerable <T> Pathfind <T>(T start, T end, ConnectivityGraph <T> graph)
{
if (start.Equals(end))
{
return(Enumerable.Empty <T>());
}
var map = CreateDijkstraMap(start, end, graph);
if (!map.Any())
{
return(Enumerable.Empty <T>()); // can't go there
}
if (map.Any(n => n.Location.Equals(end)))
{
// can reach it
var nodes = new List <PathfinderNode <T> >();
var node = map.Where(n => n.Location.Equals(end)).OrderBy(n => n.Cost).First();
while (node != null)
{
nodes.Add(node);
node = node.PreviousNode;
}
return(nodes.Select(n => n.Location).Where(s => !s.Equals(start)).Reverse());
}
else
{
// can't reach it; get as close as possible
var reverseMap = CreateDijkstraMap(end, start, graph);
var target = reverseMap.Join(map, rev => rev.Location, fwd => fwd.Location, (rev, fwd) => new { Location = rev.Location, ForwardCost = fwd.Cost, ReverseCost = rev.Cost }).WithMin(n => n.ReverseCost).WithMin(n => n.ForwardCost).FirstOrDefault();
if (target == null)
{
return(Enumerable.Empty <T>()); // can't go there
}
else
{
// go to the closest point
var nodes = new List <PathfinderNode <T> >();
var node = map.Where(n => n.Location.Equals(target.Location)).OrderBy(n => n.Cost).First();
while (node != null)
{
nodes.Add(node);
node = node.PreviousNode;
}
return(nodes.Select(n => n.Location).Where(s => !s.Equals(start)).Reverse());
}
}
}
/// <summary>
/// Connects two nodes.
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="twoWay">Connect both ways?</param>
public void Connect(T start, T end, bool twoWay = false)
{
if (!connections.ContainsKey(start) || !connections[start].Contains(end))
{
var sub1 = Subgraphs.Single(s => s.Contains(start));
var sub2 = Subgraphs.Single(s => s.Contains(end));
if (sub1 != sub2)
{
var sub3 = new ConnectivityGraph <T>(sub1, sub2);
subgraphs.Remove(sub1);
subgraphs.Remove(sub2);
subgraphs.Add(sub3);
}
connections[start].Add(end);
singletons.Remove(start);
singletons.Remove(end);
}
if (twoWay)
{
Connect(end, start, false);
}
}
public static IEnumerable <PathfinderNode <T> > CreateDijkstraMap <T>(T start, T end, ConnectivityGraph <T> graph)
{
// pathfind!
// step 1: empty priority queue with cost to reach each node
var queue = new List <PathfinderNode <T> >();
// step 2: empty set of previously visited nodes, along with costs and previous-node references
var visited = new SafeDictionary <T, PathfinderNode <T> >();
// step 3: add start node and cost
queue.Add(new PathfinderNode <T>(start, 0, null));
// step 4: quit if there are no nodes (all paths exhausted without finding goal)
bool success = false;
while (queue.Any() && !success)
{
// step 5: take lowest cost node out of queue
// also prefer straight line movement to diagonal
var minCost = queue.Min(n => n.Cost);
var node = queue.Where(n => n.Cost == minCost).First();
queue.Remove(node);
// step 6: if node is the goal, stop - success!
if (node.Location.Equals(end))
{
success = true;
}
// step 7: check possible moves
var moves = graph.GetExits(node.Location);
// step 7a: remove blocked points (aka calculate cost)
// nothing to do here
// step 7b: update priority queue
foreach (var move in moves)
{
if (!visited.ContainsKey(move))
{
// didn't visit yet
var newnode = new PathfinderNode <T>(move, node.Cost + 1, node);
queue.Add(newnode);
visited.Add(move, newnode);
}
else
{
// already visited - but is it by a longer path?
var items = queue.Where(n => n.Location.Equals(move) && n.Cost > node.Cost + 1);
if (items.Any())
{
foreach (var old in items.ToArray())
{
queue.Remove(old);
}
var newnode = new PathfinderNode <T>(move, node.Cost + 1, node);
queue.Add(newnode);
visited.Add(move, newnode);
}
}
}
}
return(visited.Values);
}
