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); }
public override bool Equals(object obj) { VectorVertex v = obj as VectorVertex; return(v != null && v.GetHashCode() == this.GetHashCode() && (v.Value.X == this.Value.X && v.Value.Y == this.Value.Y)); }