public static List <Graph.GraphNode> FindPath(Graph.GraphNode start, Graph.GraphNode goal, Pathtype pathtype) { Queue <Graph.GraphNode> toExplore = new Queue <Graph.GraphNode>(); toExplore.Enqueue(goal); Graph.GraphNode currentNode = toExplore.Peek(); while (toExplore.Count > 0) { foreach (Graph.Connection connection in currentNode.connections) { switch (pathtype) { case Pathtype.SHORTEST: if (connection.neighbor.weight < currentNode.weight + connection.distance) { connection.neighbor.weight = currentNode.weight + connection.distance; connection.neighbor.previous = currentNode; if (currentNode != start) { toExplore.Enqueue(connection.neighbor); } } break; case Pathtype.FASTEST: if (connection.neighbor.weight < currentNode.weight + connection.timeNeeded) { connection.neighbor.weight = currentNode.weight + connection.timeNeeded; connection.neighbor.previous = currentNode; if (currentNode != start) { toExplore.Enqueue(connection.neighbor); } } break; } } currentNode = toExplore.Dequeue(); } List <Graph.GraphNode> path = new List <Graph.GraphNode>(); while (currentNode.previous != null) { path.Add(currentNode); currentNode = currentNode.previous; } return(path); }
public Bitmap DrawMap(Graph mapGraph, Hashtable pens, int renderWidth, int renderHeight) { Graph.GraphNode[] nodes = mapGraph.GetNodes(); int nodesPerThread = (int)Math.Ceiling(nodes.Length / (double)threads); int activeThreads = threads; int renderedNodes = 0; ConcurrentQueue <Line> draw = new ConcurrentQueue <Line>(); Bitmap render = new Bitmap(renderWidth, renderHeight); Console.WriteLine("Drawing Map..."); for (int thread = 0; thread < threads; thread++) { int startNodeIndex = thread * nodesPerThread; int maxNodeIndex = (thread + 1) * nodesPerThread; new Thread(delegate() { for (; startNodeIndex < maxNodeIndex && startNodeIndex < nodes.Length; startNodeIndex++) { Graph.GraphNode node = nodes[startNodeIndex]; if (Functions.DistanceBetweenNodes(renderCenter, node) * scale < (renderHeight > renderWidth ? renderHeight : renderWidth)) { foreach (Graph.Connection connection in node.connections) { Pen pen = pens[connection.roadType] == null ? (Pen)pens["default"] : (Pen)pens[connection.roadType]; _2DNode _2dfrom = this.GetCoordinatesFromCenter(node.coordinates); _2DNode _2dto; foreach (_3DNode coord in connection.coordinates) { _2dto = this.GetCoordinatesFromCenter(coord); draw.Enqueue(new Line(pen, new _2DNode(_2dfrom.X + (renderWidth / 2), _2dfrom.Y + (renderHeight / 2)), new _2DNode(_2dto.X + (renderWidth / 2), _2dto.Y + (renderHeight / 2)))); _2dfrom = _2dto; } _2dto = this.GetCoordinatesFromCenter(connection.neighbor.coordinates); draw.Enqueue(new Line(pen, new _2DNode(_2dfrom.X + (renderWidth / 2), _2dfrom.Y + (renderHeight / 2)), new _2DNode(_2dto.X + (renderWidth / 2), _2dto.Y + (renderHeight / 2)))); } renderedNodes++; } } activeThreads--; }).Start(); } Console.WriteLine("Total Nodes: {0}", nodes.Length); using (Graphics g = Graphics.FromImage(render)) { while (activeThreads > 0 || draw.Count > 0) { if (draw.Count > 0) { Line line; while (!draw.TryDequeue(out line)) { ; } float halfPenWidth = line.pen.Width / 2; g.FillEllipse(new SolidBrush(line.pen.Color), line.from.X - halfPenWidth, line.from.Y - halfPenWidth, line.pen.Width, line.pen.Width); g.DrawLine(line.pen, line.from.X, line.from.Y, line.to.X, line.to.Y); g.FillEllipse(new SolidBrush(line.pen.Color), line.to.X - halfPenWidth, line.to.Y - halfPenWidth, line.pen.Width, line.pen.Width); } } Console.WriteLine(string.Format("Done :) Total/Rendered Nodes: {0}/{1}", nodes.Length, renderedNodes)); return(render); } }
public static double DistanceBetweenNodes(_3DNode node1, Graph.GraphNode node2) { return(DistanceBetweenCoordinates(node1.lat, node1.lon, node2.coordinates.lat, node2.coordinates.lon)); }
public Graph ImportOSM(string path) { Graph retGraph = new Graph(); this.OnProgress?.Invoke(this, new ProgressEventArgs { progress = 0 }); this.OnStatusChange?.Invoke(this, new StatusChangedEventArgs { status = "Importing Nodes and Counting Occurances..." }); Dictionary <ulong, uint> nodeOccurances = new Dictionary <ulong, uint>(); Hashtable allNodes = new Hashtable(); XmlReaderSettings readerSettings = new XmlReaderSettings() { IgnoreWhitespace = true }; using (XmlReader reader = XmlReader.Create(path, readerSettings)) { reader.MoveToContent(); while (reader.Read()) { if (reader.NodeType != XmlNodeType.EndElement) { switch (reader.Name) { case "node": ulong id = Convert.ToUInt64(reader.GetAttribute("id")); float lat = Convert.ToSingle(reader.GetAttribute("lat").Replace(".", ",")); float lon = Convert.ToSingle(reader.GetAttribute("lon").Replace(".", ",")); allNodes.Add(id, new Graph.GraphNode(id, lat, lon)); break; case "bounds": retGraph.minLat = Convert.ToSingle(reader.GetAttribute("minlat").Replace(".", ",")); retGraph.minLon = Convert.ToSingle(reader.GetAttribute("minlon").Replace(".", ",")); retGraph.maxLat = Convert.ToSingle(reader.GetAttribute("maxlat").Replace(".", ",")); retGraph.maxLon = Convert.ToSingle(reader.GetAttribute("maxlon").Replace(".", ",")); break; case "nd": ulong nodeID = Convert.ToUInt64(reader.GetAttribute("ref")); if (!nodeOccurances.ContainsKey(nodeID)) { nodeOccurances.Add(nodeID, 1); } else { nodeOccurances[nodeID]++; } break; } } } reader.Close(); } Hashtable speeds = new Hashtable(); foreach (string speed in File.ReadAllLines("speeds.txt")) { speeds.Add(speed.Split(',')[0], Convert.ToInt32(speed.Split(',')[1])); } List <string> copykeys = new List <string>(); foreach (string key in File.ReadAllLines("copykeys.txt")) { copykeys.Add(key); } List <Graph.GraphNode> nodes = new List <Graph.GraphNode>(); Dictionary <string, string> tags = new Dictionary <string, string>(); this.OnStatusChange?.Invoke(this, new StatusChangedEventArgs { status = "Splitting and Importing Ways" }); NodeType nodeType = NodeType.UNKNOWN; using (XmlReader reader = XmlReader.Create(path, readerSettings)) { reader.MoveToContent(); while (reader.Read()) { if (reader.Depth == 1) { if (nodeType == NodeType.WAY) { if (tags.ContainsKey("highway") && nodes.Count > 1) { int speed = (int)speeds["default"]; if (tags.ContainsKey("maxspeed")) { try { speed = Convert.ToInt32(tags["maxspeed"]); } catch (FormatException) { Console.WriteLine("Maxspeed '{0}' not implemented", tags["maxspeed"]); } } else if (speeds.ContainsKey(tags["highway"])) { speed = (int)speeds[tags["highway"]]; } string name = ""; if (tags.ContainsKey("ref")) { name = tags["ref"]; } else if (tags.ContainsKey("name")) { name = tags["name"]; } ; Graph.GraphNode start = retGraph.GetNode(nodes[0].id); List <_3DNode> coords = new List <_3DNode>(); double distance = 0.0; for (int i = 1; i < nodes.Count - 1; i++) { if (nodeOccurances[nodes[i].id] > 1) { Graph.GraphNode intersection = retGraph.GetNode(nodes[i].id); if (!tags.ContainsKey("oneway") || tags["oneway"] == "no") { start.connections.Add(new Graph.Connection(distance, (float)distance / speed, intersection, name, coords.ToArray(), tags["highway"])); } coords.Reverse(); intersection.connections.Add(new Graph.Connection(distance, (float)distance / speed, start, name, coords.ToArray(), tags["highway"])); start = intersection; distance = 0; coords.Clear(); } else { float lat = nodes[i].coordinates.lat; float lon = nodes[i].coordinates.lon; distance += coords.Count > 0 ? Functions.DistanceBetweenCoordinates(coords[coords.Count - 1].lat, coords[coords.Count - 1].lon, lat, lon) : 0; coords.Add(new _3DNode(lat, lon)); retGraph.RemoveNode(nodes[i].id); } } Graph.GraphNode goal = retGraph.GetNode(nodes[nodes.Count - 1].id); if (!tags.ContainsKey("oneway") || tags["oneway"] == "no") { start.connections.Add(new Graph.Connection(distance, (float)distance / speed, goal, name, coords.ToArray(), tags["highway"])); } coords.Reverse(); goal.connections.Add(new Graph.Connection(distance, (float)distance / speed, start, name, coords.ToArray(), tags["highway"])); } } switch (reader.Name) { case "node": nodeType = NodeType.NODE; break; case "way": nodeType = NodeType.WAY; nodes.Clear(); tags.Clear(); break; default: nodeType = NodeType.UNKNOWN; break; } } else if (reader.Depth == 2 && nodeType == NodeType.WAY) { switch (reader.Name) { case "nd": ulong id = Convert.ToUInt64(reader.GetAttribute("ref")); if (retGraph.ContainsNode(id)) { nodes.Add(retGraph.GetNode(id)); } else if (allNodes.ContainsKey(id)) { retGraph.AddNode((Graph.GraphNode)allNodes[id]); allNodes.Remove(id); nodes.Add(retGraph.GetNode(id)); } break; case "tag": if (copykeys.Contains(reader.GetAttribute("k"))) { tags.Add(reader.GetAttribute("k"), reader.GetAttribute("v").ToString()); } break; } } } reader.Close(); } return(retGraph); }
public static List <Graph.GraphNode> FindShortestPath(Graph.GraphNode start, Graph.GraphNode goal) { return(FindPath(start, goal, Pathtype.SHORTEST)); }
public static List <Graph.GraphNode> FindQuickestPath(Graph.GraphNode start, Graph.GraphNode goal) { return(FindPath(start, goal, Pathtype.FASTEST)); }