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));
}
