public bool SetDistance(Node neighbor, float distance)
{
try
{
if (!neighborDistances.ContainsKey(neighbor))
{
return(false);
}
neighborDistances[neighbor] = distance;
}
catch (ArgumentException e)
{
ZLogger.Message($"Set Distance: Key already exists: (I think... e.message = {e.Message}. Would be key was node for {neighbor.key}", debugLevel: DebugLevel.Pathfinding);
}
return(true);
}
public bool AddNeighbor(Node neighbor, float cost = Single.PositiveInfinity)
{
try
{
if (!neighbors.Add(neighbor))
{
return(false);
}
neighbor.AddNeighbor(this, cost);
neighborDistances.SetOrAdd(neighbor, cost);
}
catch (ArgumentException e)
{
ZLogger.Message(
$"AddNeighbor: Key already exists: (I think... e.message = {e.Message}. Would be key was node for {neighbor.key}", debugLevel: DebugLevel.Pathfinding);
}
return(true);
}
private List <Node> DijkstraConnect(Node source, Node sink, out float routeCost)
{
string lastKey = "nokey";
routeCost = -1;
StringBuilder sb = new StringBuilder();
try
{
List <Node> ret = null;
Dictionary <Node, float> distances = new Dictionary <Node, float>(nodes.Count);
Dictionary <Node, Node> previousNodes = new Dictionary <Node, Node>(nodes.Count);
HashSet <Node> Q = new HashSet <Node>(nodes);
Q.Add(sink);
foreach (Node nd in nodes)
{
distances.SetOrAdd(nd, Single.PositiveInfinity);
previousNodes.SetOrAdd(nd, null);
}
distances.SetOrAdd(sink, float.PositiveInfinity);
previousNodes.SetOrAdd(sink, null);
previousNodes.SetOrAdd(source, null);
ZLogger.Message("DijkstraConnect", debugLevel: DebugLevel.Pathfinding);
lastKey = "distances/source";
distances.SetOrAdd(source, 0);
foreach (Node neighbor in source.neighbors)
{
distances[neighbor] = source.neighborDistances[neighbor];
}
bool found = false;
Node u = source;
while (Q.Count > 0)
{
sb.Clear();
u = Q.MinBy((x) => distances[x]);
//foreach (Node nd in u.neighbors)
//{
// sb.Append($"nd = {nd}, distance = {distances[nd]}");
//}
sb.Append($"Chose {u} as shortest with {distances[u]}- looking for sink {sink}");
ZLogger.Message(sb.ToString(), debugLevel: DebugLevel.Pathfinding);
//Console.WriteLine(sb.ToString());
Q.Remove(u);
if (u == sink)
{
break;
}
foreach (Node v in u.neighbors)
{
float altDistance = distances[u] + u.neighborDistances[v];
if (altDistance < distances[v])
{
distances[v] = altDistance;
previousNodes[v] = u;
}
}
}
sb.Clear();
foreach (Node node in previousNodes.Keys)
{
sb.Append($"Node:{node}, previous: {previousNodes[node]}, cumulative distance: {distances[node]}");
}
ret = new List <Node>();
routeCost = distances[sink];
sb.Clear();
while (sink != null)
{
sb.AppendLine($"{sink}");
ret.Add(sink);
sink = previousNodes[sink];
}
ret.Add(source);
sb.AppendLine($"Source Key = {source}");
ZLogger.Message(sb.ToString(), debugLevel: DebugLevel.Pathfinding);
ret.Reverse();
return(ret);
}
catch (Exception e)
{
sb = new StringBuilder();
foreach (object v in e.Data.Keys)
{
sb.Append($"{e.Data[v]}: {v}");
}
ZLogger.Message($"Couldn't find key! Or key was null... {lastKey} Exception data of type {e.GetType()} follows: {sb}", debugLevel: DebugLevel.Pathfinding);
return(new List <Node>());
}
}