protected Func <TVertex, double> DistancesIndexGetter()
{
return(AlgorithmExtensions.GetIndexer <TVertex, double>(this.distances));
}
internal ILineString PerformShortestPathAnalysis(IPoint source, IPoint destination, bool usesCondensedGraph)
{
// We keep a copy so we can terminate the search early.
_theDestination = destination;
// This is an instrance of the shortest path algortihm.
_dijkstra = new DijkstraShortestPathAlgorithm <Coordinate, Edge <Coordinate> >(_graph, AlgorithmExtensions.GetIndexer(_edgeCost));
// Quick Graph uses 'observers' to record the distance traveled and the path travelled througth,
var distObserver = new VertexDistanceRecorderObserver <Coordinate, Edge <Coordinate> >(AlgorithmExtensions.GetIndexer(_edgeCost));
var predecessorObserver = new VertexPredecessorRecorderObserver <Coordinate, Edge <Coordinate> >();
distObserver.Attach(_dijkstra);
predecessorObserver.Attach(_dijkstra);
// Having this present means that when we finally reach the target node
// the dijkstra algortihm can quit without scanning other nodes in the graph, leading to
// performance increases.
_dijkstra.FinishVertex += dijkstra_FinishVertex;
// This is where the shortest path is calculated.
var sw = new System.Diagnostics.Stopwatch();
sw.Start();
_dijkstra.Compute(source.Coordinate);
sw.Stop();
System.Diagnostics.Debug.WriteLine("Dijsktra Took: " + sw.ElapsedMilliseconds);
// get the cost of the path. If one is found then d (=distance) should be greater than zero so we get the edges making up
// the path.
double d = AlgorithmExtensions.ComputePredecessorCost(predecessorObserver.VertexPredecessors, _edgeCost, destination.Coordinate);
System.Diagnostics.Debug.WriteLine(d);
if (d > 0)
{
IEnumerable <Edge <Coordinate> > edgesInShortestPath;
if (predecessorObserver.TryGetPath(destination.Coordinate, out edgesInShortestPath))
{
var theCompleteShortestPath = new List <Coordinate>();
// We need to use a different approach when using the condensed graph.
if (usesCondensedGraph)
{
foreach (var edgeInShortPath in edgesInShortestPath)
{
var ls = GetLineStringInformation(edgeInShortPath);
if (ls != null)
{
// We need to get each of the nodes that makes up the lines.
// we need to add each of them on one list.
var count = ls.Coordinates.Length;
for (var i = 0; i < count; i++)
{
theCompleteShortestPath.Add(ls.Coordinates[i]);
}
} // End Of If
} // Loop Around Each Edge In The Shortest Path
} // End of If
else
{
foreach (var edgeInShortPath in edgesInShortestPath)
{
theCompleteShortestPath.Add(edgeInShortPath.Source);
theCompleteShortestPath.Add(edgeInShortPath.Target);
}
} // End Of Else
var theShortestPath = _geomFactory.CreateLineString(theCompleteShortestPath.ToArray());
return(theShortestPath);
} // There Was A Shortest Path
// Return null.
// ToDo: Need to improve this bit so it at least indicates if the SP didnt get a path/
return(null);
}
return(null);
}
public static void Create(List <Node> Nodes, List <Way> Ways)
{
//create the graph
var Graph = new BidirectionalGraph <int, CompEdge>();
//add each node referenced by a way
foreach (Way Path in Ways)
{
for (int i = 0; i < Path.References.Count; i++)
{
int Id = (int)Path.References[i].Id;
// make sure the node was not already added
if (!Graph.Vertices.Contains(Id))
{
Graph.AddVertex(Id);
short Altitude = AltitudeMap.AltitudeAtPosition(Path.References[i].Location);
NodeAltitudes.Add(Id, (int)Altitude);
}
}
}
//create an altitude map
Bitmap AltitudeImage = new Bitmap(64, 64, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
List <double> LongitudeGradient = Gradient(Program.LowerBound.Longitude, Program.UpperBound.Longitude, 64);
List <double> LatitudeGradient = Gradient(Program.LowerBound.Latitude, Program.UpperBound.Latitude, 64);
//iterate over altitudes
for (int longitudeindex = 0; longitudeindex < LongitudeGradient.Count; longitudeindex++)
{
for (int latitudeindex = 0; latitudeindex < LatitudeGradient.Count; latitudeindex++)
{
short Altitude = AltitudeMap.AltitudeAtPosition(new GeoCoordinate(LatitudeGradient[latitudeindex], LongitudeGradient[longitudeindex]));
AltitudeImage.SetPixel(longitudeindex, latitudeindex, Color.FromArgb(Altitude, Altitude, Altitude));
}
}
// setup the bitmap that all of the data will be saved to
AltitudeImage.Save("RegionalAltitudeImage.bmp");
//add each edge
foreach (Way Path in Ways)
{
//add all of the edges
for (int i = 1; i < Path.References.Count; i++)
{
Node FirstNode = Path.References[i - 1];
Node SecondNode = Path.References[i];
//add the edge
Graph.AddEdge(new CompEdge((int)FirstNode.Id, (int)SecondNode.Id));
Graph.AddEdge(new CompEdge((int)SecondNode.Id, (int)FirstNode.Id));
//add the distance edge
double Distance = DistanceBetweenPlaces(FirstNode.Location, SecondNode.Location);
//add both distances
Distances.Add(new CompEdge((int)FirstNode.Id, (int)SecondNode.Id), Distance);
Distances.Add(new CompEdge((int)SecondNode.Id, (int)FirstNode.Id), Distance);
//get the altitudes
double FirstNodeAltitude = NodeAltitudes[(int)FirstNode.Id];
double SecondNodeAltitude = NodeAltitudes[(int)SecondNode.Id];
//if we don't know either of the altitudes, assume 0 change
if (FirstNodeAltitude != -1 || SecondNodeAltitude != -1)
{
AltitudeChange.Add(new CompEdge((int)FirstNode.Id, (int)SecondNode.Id), FirstNodeAltitude - SecondNodeAltitude);
AltitudeChange.Add(new CompEdge((int)SecondNode.Id, (int)FirstNode.Id), SecondNodeAltitude - FirstNodeAltitude);
}
else
{
AltitudeChange.Add(new CompEdge((int)FirstNode.Id, (int)SecondNode.Id), 0);
AltitudeChange.Add(new CompEdge((int)SecondNode.Id, (int)FirstNode.Id), 0);
}
}
}
//calcuate costs for each edge
var Costs = new Dictionary <CompEdge, double>();
//cycle through each edge and calculate its cost
foreach (Edge <int> Edge in Graph.Edges)
{
CompEdge Key = new CompEdge(Edge.Source, Edge.Target);
double DistanceCost = Distances[Key] * 1000; //convert to meters
double AltitudeCost = Math.Pow(AltitudeChange[Key], 2); //square the altitude (in meters)
double Weight = DistanceCost;
Costs.Add(Key, Weight);
}
//create arrays that the data will be stored in
string[] NodeLines = new string[Graph.Vertices.Count()];
string[] EdgeLines = new string[Graph.Edges.Count()];
//save nodes to a .csv file
for (int i = 0; i < Graph.Vertices.Count(); i++)
{
int Id = Graph.Vertices.ElementAt(i);
NodeLines[i] = Id + "," + Node.GetById(Id).Location.Latitude + "," + Node.GetById(Id).Location.Longitude;
}
//save edges to a .csv file
for (int i = 0; i < Graph.Edges.Count(); i++)
{
int Source = Graph.Edges.ElementAt(i).Source;
int Target = Graph.Edges.ElementAt(i).Target;
EdgeLines[i] = Source + "," + Target + "," + Costs[new CompEdge(Source, Target)];
}
File.WriteAllLines("nodes.csv", NodeLines);
File.WriteAllLines("edges.csv", EdgeLines);
int From = 0;
int To = 5;
var CostIndexer = AlgorithmExtensions.GetIndexer(Costs);
var tryGetPath = Graph.ShortestPathsDijkstra(CostIndexer, From);
IEnumerable <CompEdge> path;
if (tryGetPath(To, out path))
{
string ToWrite = "";
foreach (CompEdge Edge in path)
{
ToWrite += Convert.ToString(Edge.Source) + ",";
}
File.WriteAllText("chosenpath.csv", ToWrite.Remove(ToWrite.Length - 1)); // remove the last comma
}
//create a visualizer for the graph
//GraphvizAlgorithm<int, CompEdge> graphviz = new GraphvizAlgorithm<int, CompEdge>(Graph);
//string output = "output.dot";
//graphviz.Generate(new FileDotEngine(), output);
// assumes dot.exe is on the path:
//var args = string.Format(@"{0} -Tjpg -O", output);
//System.Diagnostics.Process.Start(@"C:\Program Files (x86)\Graphviz2.38\bin\dot.exe", args);
}
public List <FloorTile> getPath()
{
List <FloorTile> retval = new List <FloorTile>();
if (this.fp == null || this.fp.getStartTile() == null || this.fp.getTargetTile() == null)
{
return(retval);
}
startPoint = this.fp.getStartTile().Position.X + "_" + this.fp.getStartTile().Position.Y;
targetPoint = this.fp.getTargetTile().Position.X + "_" + this.fp.getTargetTile().Position.Y;
this.messages += "- Start Get Path\n";
//startPoint = txtStartPoint.Text;
//targetPoint = txtTargetPoint.Text;
DijkstraShortestPathAlgorithm <string, Edge <string> > dijkstra = new DijkstraShortestPathAlgorithm <string, Edge <string> >(graph, AlgorithmExtensions.GetIndexer <Edge <string>, double>(edgeCost));
// Attach a Vertex Predecessor Recorder Observer to give us the paths
QuickGraph.Algorithms.Observers.VertexPredecessorRecorderObserver <string, Edge <string> > predecessorObserver = new QuickGraph.Algorithms.Observers.VertexPredecessorRecorderObserver <string, Edge <string> >();
predecessorObserver.Attach(dijkstra);
// attach a distance observer to give us the shortest path distances
VertexDistanceRecorderObserver <string, Edge <string> > distObserver = new VertexDistanceRecorderObserver <string, Edge <string> >(AlgorithmExtensions.GetIndexer <Edge <string>, double>(edgeCost));
distObserver.Attach(dijkstra);
// Run the algorithm with A set to be the source
dijkstra.Compute(startPoint);
this.messages += " Start Point: " + startPoint + ".\n";
this.messages += " Target Point: " + targetPoint + ".\n";
String outString = "";
//outString += distObserver.Distances[targetPoint] + "\n";
IEnumerable <Edge <string> > path;
if (predecessorObserver.TryGetPath(targetPoint, out path))
{
foreach (var u in path)
{
outString += u + ";";
}
}
string[] outEdges = Regex.Split(outString, ";");
if (outEdges.Length > 0)
{
for (int i = 0; i < outEdges.Length; i++)
{
if (outEdges[i].Length > 0)
{
this.messages += outEdges[i] + "\n";
string[] outPoint = Regex.Split(outEdges[i], "->");
//start points
retval.Add(getTileByIndex(fp, outPoint[0]));
}
}
//add target
retval.Add(getTileByIndex(fp, targetPoint));
}
this.messages += retval.Count.ToString() + "\n";;
if (retval.Count == 1 && retval[0].Equals(getTileByIndex(fp, targetPoint)))
{
this.messages += "Can't find path. Start or end point is not walkable or no available walkable tiles" + "\n";;
return(null);
}
fp.setPath(retval);
//return retval;
return(condenseList(retval));
}
public void DijkstraSimpleGraph2()
{
var graph = new AdjacencyGraph <char, Edge <char> >();
var distances = new Dictionary <Edge <char>, double>();
graph.AddVertexRange("ABCDE");
AddEdge('A', 'C', 1);
AddEdge('B', 'B', 2);
AddEdge('B', 'D', 1);
AddEdge('B', 'E', 2);
AddEdge('C', 'B', 7);
AddEdge('C', 'D', 3);
AddEdge('D', 'E', 1);
AddEdge('E', 'A', 1);
AddEdge('E', 'B', 1);
var algorithm = new DijkstraShortestPathAlgorithm <char, Edge <char> >(graph, AlgorithmExtensions.GetIndexer(distances));
var predecessors = new VertexPredecessorRecorderObserver <char, Edge <char> >();
using (predecessors.Attach(algorithm))
algorithm.Compute('A');
Assert.AreEqual(0, algorithm.GetDistance('A'));
Assert.AreEqual(6, algorithm.GetDistance('B'));
Assert.AreEqual(1, algorithm.GetDistance('C'));
Assert.AreEqual(4, algorithm.GetDistance('D'));
Assert.AreEqual(5, algorithm.GetDistance('E'));
#region Local function
void AddEdge(char source, char target, double weight)
{
var edge = new Edge <char>(source, target);
distances[edge] = weight;
graph.AddEdge(edge);
}
#endregion
}
public static void ValidatePathsUsingDijkstra(Topology.IGP.Topology igp_topology, Topology.MPLS.HierarchicalLabelSwitchedPath.HierarchicalLabelSwitchedPath hlsp)
{
Console.WriteLine("\n====Validating Paths====\n");
List <yggdrasil2.Topology.Node.Node> nodes_copy = igp_topology.Nodes.DeepClone();
List <yggdrasil2.Topology.IGP.Link.Link> links_copy = igp_topology.Links.DeepClone();
BidirectionalGraph <string, TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link> > graph =
new BidirectionalGraph <string, TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link> >();
Dictionary <TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link>, double> cost =
new Dictionary <TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link>, double>();
var start = nodes_copy.Where(n => n.IPv4RouterIdentifier == hlsp.IPv4TunnelSenderAddress).SingleOrDefault();
var end = nodes_copy.Where(n => n.IPv4RouterIdentifier == hlsp.IPv4TunnelEndpointAddress).SingleOrDefault();
if (start != null && end != null)
{
foreach (var lsp in hlsp.Children)
{
graph.Clear();
foreach (var node in nodes_copy)
{
graph.AddVertex(node.Id);
}
if (!start.IsPseudonode) // it will never be a pseudonode, get rid of this
{
var nodeLinks = links_copy.Where(l => l.SourceNode == start.Id).ToList();
foreach (var l in nodeLinks)
{
if (!graph.ContainsEdge(l.SourceNode, l.TargetNode))
{
if (l.OperationalStatus)
{
var forwardEdge = new TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link>(l.SourceNode, l.TargetNode, l);
graph.AddEdge(forwardEdge);
cost.Add(forwardEdge, 1);
}
}
}
}
foreach (var hop in lsp.ComputedExplicitRouteObject)
{
var link = links_copy.Where(l => l.IPv4InterfaceAddress == hop).SingleOrDefault();
if (link != null)
{
if (!graph.ContainsEdge(link.SourceNode, link.TargetNode))
{
if (link.OperationalStatus)
{
var backwardEdge = new TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link>(link.TargetNode, link.SourceNode, link);
graph.AddEdge(backwardEdge);
cost.Add(backwardEdge, 1);
}
//var srcNode = nodes_copy.Where(n => n.IgpRouterIdentifier == link.SourceNode).SingleOrDefault();
var dstNode = nodes_copy.Where(n => n.IgpRouterIdentifier == link.TargetNode).SingleOrDefault();
//if (srcNode != null)
//{
// if (srcNode.IsPseudonode)
// {
// var nodeLinks = links_copy.Where(l => l.TargetNode == srcNode.Id).ToList();
// foreach (var l in nodeLinks)
// {
// if (!graph.ContainsEdge(l.SourceNode, l.TargetNode))
// {
// if (l.OperationalStatus)
// {
// var forwardEdge = new TaggedEdge<string, yggdrasil2.Topology.IGP.Link.Link>(l.SourceNode, l.TargetNode, l);
// graph.AddEdge(forwardEdge);
// cost.Add(forwardEdge, 1);
// }
// }
// }
// }
//}
if (dstNode != null)
{
if (dstNode.IsPseudonode)
{
var nodeLinks = links_copy.Where(l => l.TargetNode == dstNode.Id).ToList();
foreach (var l in nodeLinks)
{
if (!graph.ContainsEdge(l.SourceNode, l.TargetNode))
{
if (l.OperationalStatus)
{
var forwardEdge = new TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link>(l.SourceNode, l.TargetNode, l);
graph.AddEdge(forwardEdge);
cost.Add(forwardEdge, 1);
}
}
}
}
}
}
}
}
DijkstraShortestPathAlgorithm <string, TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link> > dijkstra =
new DijkstraShortestPathAlgorithm <string, TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link> >(graph,
AlgorithmExtensions.GetIndexer <TaggedEdge <string, yggdrasil2.Topology.IGP.Link.Link>, double>(cost));
dijkstra.Compute(start.Id);
if (dijkstra.Distances.ContainsKey(end.Id) && dijkstra.Distances[end.Id] != double.MaxValue)
{
lsp.Feasible = true;
Console.WriteLine("Path {0} is \u001b[32mFEASIBLE\u001b[0m.\n\t{1} is REACHABLE from {2} in {3} hops (includes pseudonodes).",
lsp.SymbolicPathName, lsp.IPv4TunnelEndpointAddress, lsp.IPv4TunnelSenderAddress, dijkstra.Distances[end.Id]);
}
else
{
lsp.Feasible = false;
Console.WriteLine("Path {0} is \u001b[31mNOT FEASIBLE\u001b[0m.\n\t{1} is UREACHABLE from {2}.",
lsp.SymbolicPathName, lsp.IPv4TunnelEndpointAddress, lsp.IPv4TunnelSenderAddress);
}
}
}
}
public void Compute()
{
var g = new AdjacencyGraph <char, Edge <char> >();
var distances = new Dictionary <Edge <char>, double>();
g.AddVertexRange("ABCDE");
AddEdge(g, distances, 'A', 'C', 1);
AddEdge(g, distances, 'B', 'B', 2);
AddEdge(g, distances, 'B', 'D', 1);
AddEdge(g, distances, 'B', 'E', 2);
AddEdge(g, distances, 'C', 'B', 7);
AddEdge(g, distances, 'C', 'D', 3);
AddEdge(g, distances, 'D', 'E', 1);
AddEdge(g, distances, 'E', 'A', 1);
AddEdge(g, distances, 'E', 'B', 1);
var dijkstra = new DijkstraShortestPathAlgorithm <char, Edge <char> >(g, AlgorithmExtensions.GetIndexer(distances));
var predecessors = new VertexPredecessorRecorderObserver <char, Edge <char> >();
using (predecessors.Attach(dijkstra))
dijkstra.Compute('A');
Assert.AreEqual(0, dijkstra.Distances['A']);
Assert.AreEqual(6, dijkstra.Distances['B']);
Assert.AreEqual(1, dijkstra.Distances['C']);
Assert.AreEqual(4, dijkstra.Distances['D']);
Assert.AreEqual(5, dijkstra.Distances['E']);
}
static void Main(string[] args)
{
var graph = new UndirectedGraph <TaxiPoint, TaggedEdge <TaxiPoint, string> >();
// Krymsk Airfield
TaxiPoint runwayFour = new TaxiPoint("Runway 4");
TaxiPoint adJunction = new TaxiPoint("Alpha / Delta Junction");
TaxiPoint acJunction = new TaxiPoint("Alpha / Charlie Junction");
TaxiPoint bdJunction = new TaxiPoint("Bravo / Delta Junction");
TaxiPoint padsFourteenThroughTwenty = new TaxiPoint("Pads 14 to 20");
graph.AddVertex(runwayFour);
graph.AddVertex(adJunction);
graph.AddVertex(acJunction);
graph.AddVertex(bdJunction);
graph.AddVertex(padsFourteenThroughTwenty);
TaggedEdge <TaxiPoint, string> runwayFourToAdJunction = new TaggedEdge <TaxiPoint, string>(runwayFour, adJunction, "Alpha");
TaggedEdge <TaxiPoint, string> adJunctionToAcJunction = new TaggedEdge <TaxiPoint, string>(adJunction, acJunction, "Alpha");
TaggedEdge <TaxiPoint, string> adJunctionToBdJunction = new TaggedEdge <TaxiPoint, string>(adJunction, bdJunction, "Delta");
TaggedEdge <TaxiPoint, string> acJunctionToPadsFourteenThroughTwenty = new TaggedEdge <TaxiPoint, string>(acJunction, padsFourteenThroughTwenty, "Charlie");
graph.AddEdge(runwayFourToAdJunction);
graph.AddEdge(adJunctionToAcJunction);
graph.AddEdge(adJunctionToBdJunction);
graph.AddEdge(acJunctionToPadsFourteenThroughTwenty);
Dictionary <TaggedEdge <TaxiPoint, string>, double> edgeCostDictionary = new Dictionary <TaggedEdge <TaxiPoint, string>, double>(graph.EdgeCount)
{
{ runwayFourToAdJunction, 1 },
{ adJunctionToAcJunction, 1 },
{ adJunctionToBdJunction, 1 },
{ acJunctionToPadsFourteenThroughTwenty, 1 }
};
string dotGraph = graph.ToGraphviz(algorithm =>
{
// Custom init example
algorithm.FormatVertex += (sender, vertexArgs) =>
{
vertexArgs.VertexFormat.Label = $"{vertexArgs.Vertex.Name}";
};
algorithm.FormatEdge += (sender, edgeArgs) =>
{
var label = new QuikGraph.Graphviz.Dot.GraphvizEdgeLabel
{
Value = $"Taxiway {edgeArgs.Edge.Tag} : Cost {edgeCostDictionary[edgeArgs.Edge]}"
};
edgeArgs.EdgeFormat.Label = label;
};
});
Console.WriteLine("Graph Definition");
Console.WriteLine("-------------------------------------");
Console.WriteLine(dotGraph);
Console.WriteLine("-------------------------------------");
Console.WriteLine("Shortest Path Test");
TaxiPoint root = padsFourteenThroughTwenty;
Func <TaggedEdge <TaxiPoint, string>, double> edgeCost = AlgorithmExtensions.GetIndexer(edgeCostDictionary);
TryFunc <TaxiPoint, IEnumerable <TaggedEdge <TaxiPoint, string> > > tryGetPaths = graph.ShortestPathsDijkstra(edgeCost, root);
// Query path for given vertices
TaxiPoint target = runwayFour;
Console.WriteLine("Getting Path");
if (tryGetPaths(target, out IEnumerable <TaggedEdge <TaxiPoint, string> > path))
{
List <string> taxiways = new List <string>();
foreach (TaggedEdge <TaxiPoint, string> edge in path)
{
taxiways.Add(edge.Tag);
}
Console.WriteLine("{0} to {1} via taxiways {2}", root.Name, target.Name, string.Join(", ", RemoveRepeating(taxiways)));
}
else
{
Console.WriteLine("Path was null");
}
Console.WriteLine("Press Enter to close");
Console.ReadLine();
}
