public static Graph <VectorVertex, VectorEdge> CreateGraphFromStreetData(IEnumerable <Tuple <Vector2, Vector2> > roadData)
{
var edges = new Dictionary <VectorVertex, ISet <VectorEdge> >();
var vertexes = new Dictionary <string, VectorVertex>();
string name;
foreach (var road in roadData)
{
name = road.Item1.GetVectorName();
if (!vertexes.Keys.Contains(name))
{
vertexes.Add(name, new VectorVertex(road.Item1));
}
}
foreach (var road in roadData)
{
var source = vertexes[road.Item1.GetVectorName()];
var edge = new VectorEdge(source, vertexes[road.Item2.GetVectorName()]);
if (!edges.Keys.Contains(source))
{
edges.Add(source, new HashSet <VectorEdge>());
}
edges[source].Add(edge);
}
VectorGraph graph = new VectorGraph(edges, vertexes);
return(graph);
}
public static IEnumerable <Tuple <Vector2, Vector2> > FindRoute(Vector2 startPoint, Vector2 endPoint, IEnumerable <Tuple <Vector2, Vector2> > roadData)
{
Graph <VectorVertex, VectorEdge> graph = VectorGraph.CreateGraphFromStreetData(roadData);
VectorVertex start = new VectorVertex(startPoint);
VectorVertex end = new VectorVertex(endPoint);
VectorVertex currentNode;
Dictionary <VectorVertex, ISet <VectorEdge> > edges = new Dictionary <VectorVertex, ISet <VectorEdge> >(graph.Edges);
Dictionary <string, VectorVertex> remainingNodes = new Dictionary <string, VectorVertex>(graph.Vertexes);
Dictionary <string, VectorVertex> processedNodes = new Dictionary <string, VectorVertex>(graph.Vertexes.Count);
foreach (var vertex in remainingNodes.Values)
{
vertex.Distance = double.PositiveInfinity;
}
remainingNodes[start.Name].Distance = 0;
currentNode = start;
while (!currentNode.Equals(end))
{
currentNode = remainingNodes.OrderBy(n => n.Value.Distance).FirstOrDefault(n => n.Value.Distance != double.PositiveInfinity).Value;
if (currentNode != null)
{
foreach (var edge in edges[currentNode])
{
if (remainingNodes.ContainsKey(edge.End.Name))
{
var adjacendNode = remainingNodes[edge.End.Name];
var newDistance = currentNode.Distance + edge.GetWeight();
if (newDistance < adjacendNode.Distance)
{
adjacendNode.Distance = newDistance;
adjacendNode.Previous = currentNode;
}
}
}
remainingNodes.Remove(currentNode.Name);
processedNodes.Add(currentNode.Name, currentNode);
}
}
currentNode = processedNodes[end.Name];
List <Tuple <Vector2, Vector2> > pathList = new List <Tuple <Vector2, Vector2> >();
while (!currentNode.Equals(start))
{
pathList.Add(new Tuple <Vector2, Vector2>(currentNode.Previous.Value, currentNode.Value));
currentNode = currentNode.Previous;
}
pathList.Reverse();
return(pathList);
}
