commit
f4d2e3dacd
@ -1,3 +1,4 @@
|
||||
Logging.Logger logger = new (Logging.LogType.CONSOLE, Logging.LogLevel.DEBUG);
|
||||
Dictionary<UInt64, Graph.Node> nodes = OpenStreetMap_Importer.Importer.Import(logger);
|
||||
Dictionary<UInt64, Graph.Node> nodes = OpenStreetMap_Importer.Importer.Import(@"", logger);
|
||||
logger.level = Logging.LogLevel.VERBOSE;
|
||||
astar.Astar astar = new(nodes, logger);
|
@ -7,10 +7,16 @@ namespace OpenStreetMap_Importer
|
||||
public class Importer
|
||||
{
|
||||
|
||||
public static Dictionary<ulong, Node> Import(Logger ?logger = null)
|
||||
public static Dictionary<ulong, Node> Import(string filePath = "", Logger ?logger = null)
|
||||
{
|
||||
List<Way> ways = new();
|
||||
Dictionary<ulong, Node> nodes = new();
|
||||
Stream mapData;
|
||||
XmlReaderSettings readerSettings = new()
|
||||
{
|
||||
IgnoreWhitespace = true,
|
||||
IgnoreComments = true
|
||||
};
|
||||
|
||||
bool wayTag = false;
|
||||
Way currentWay = new();
|
||||
@ -21,13 +27,21 @@ namespace OpenStreetMap_Importer
|
||||
* Import "ways" with a tag "highway"
|
||||
* Count occurances of "nodes" to find junctions
|
||||
*/
|
||||
XmlReaderSettings readerSettings = new()
|
||||
|
||||
if (!File.Exists(filePath))
|
||||
{
|
||||
IgnoreWhitespace = true,
|
||||
IgnoreComments = true
|
||||
};
|
||||
XmlReader reader = XmlReader.Create(new MemoryStream(OSM_Data.map), readerSettings);
|
||||
mapData = new MemoryStream(OSM_Data.map);
|
||||
logger?.Log(LogLevel.INFO, "Filepath '{0}' does not exist. Using standard file.", filePath);
|
||||
}
|
||||
else
|
||||
{
|
||||
mapData = new FileStream(filePath, FileMode.Open, FileAccess.Read);
|
||||
logger?.Log(LogLevel.INFO, "Using file '{0}'", filePath);
|
||||
}
|
||||
XmlReader reader = XmlReader.Create(mapData, readerSettings);
|
||||
reader.MoveToContent();
|
||||
|
||||
logger?.Log(LogLevel.INFO, "Importing ways and counting nodes...");
|
||||
while (reader.Read())
|
||||
{
|
||||
if (reader.Name == "way" && reader.IsStartElement())
|
||||
@ -91,16 +105,28 @@ namespace OpenStreetMap_Importer
|
||||
}
|
||||
}
|
||||
|
||||
logger?.Log(LogLevel.DEBUG, "Ways: {0} Nodes: {1}", ways.Count, nodes.Count);
|
||||
logger?.Log(LogLevel.DEBUG, "Loaded Ways: {0} Required Nodes: {1}", ways.Count, count.Count);
|
||||
|
||||
reader.Close();
|
||||
reader = XmlReader.Create(new MemoryStream(OSM_Data.map), readerSettings);
|
||||
GC.Collect();
|
||||
if (!File.Exists(filePath))
|
||||
{
|
||||
mapData = new MemoryStream(OSM_Data.map);
|
||||
logger?.Log(LogLevel.INFO, "Filepath '{0}' does not exist. Using standard file.", filePath);
|
||||
}
|
||||
else
|
||||
{
|
||||
mapData = new FileStream(filePath, FileMode.Open, FileAccess.Read);
|
||||
logger?.Log(LogLevel.INFO, "Using file '{0}'", filePath);
|
||||
}
|
||||
reader = XmlReader.Create(mapData, readerSettings);
|
||||
reader.MoveToContent();
|
||||
|
||||
/*
|
||||
* Second iteration
|
||||
* Import nodes that are needed by the "ways"
|
||||
*/
|
||||
logger?.Log(LogLevel.INFO, "Importing nodes...");
|
||||
while (reader.Read())
|
||||
{
|
||||
if (reader.Name == "node")
|
||||
@ -117,105 +143,85 @@ namespace OpenStreetMap_Importer
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
logger?.Log(LogLevel.INFO, "Import finished. Calculating distances.");
|
||||
|
||||
/*
|
||||
* Add connections between nodes (only junctions, e.g. nodes are referenced more than once)
|
||||
* Remove non-junction nodes
|
||||
*/
|
||||
logger?.Log(LogLevel.INFO, "Calculating Edges and distances...");
|
||||
ulong edges = 0;
|
||||
foreach(Way way in ways)
|
||||
{
|
||||
Node junction1 = nodes[way.nodeIds[0]];
|
||||
Node junction2;
|
||||
double weight = 0;
|
||||
//Iterate Node-ids in current way forwards or backwards (depending on way.direction)
|
||||
if (way.direction == Way.wayDirection.forward)
|
||||
{
|
||||
for (int index = 1; index < way.nodeIds.Count - 1; index++)
|
||||
for (int index = 0; index < way.nodeIds.Count - 1; index++)
|
||||
{
|
||||
Node currentNode = nodes[way.nodeIds[index]];
|
||||
if (count[way.nodeIds[index]] > 1)
|
||||
Node nextNode = nodes[way.nodeIds[index + 1]];
|
||||
weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
|
||||
if (count[way.nodeIds[index + 1]] > 1 || index == way.nodeIds.Count - 2)
|
||||
{
|
||||
junction2 = nodes[way.nodeIds[index]];
|
||||
/*
|
||||
* If Node is referenced more than once => Junction
|
||||
* If Node is last node of way => Junction
|
||||
* Add an edge between two junctions
|
||||
*/
|
||||
junction2 = nodes[way.nodeIds[index + 1]];
|
||||
junction1.edges.Add(new Edge(junction2, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction1.lat, junction1.lon, weight, junction2.lat, junction2.lon);
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index], weight, way.nodeIds[index + 1]);
|
||||
edges++;
|
||||
|
||||
if (!way.oneway)
|
||||
{
|
||||
junction2.edges.Add(new Edge(junction1, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction2.lat, junction2.lon, weight, junction1.lat, junction1.lon);
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index + 1], weight, way.nodeIds[index]);
|
||||
edges++;
|
||||
}
|
||||
|
||||
junction1 = junction2;
|
||||
weight = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
Node nextNode = nodes[way.nodeIds[index + 1]];
|
||||
weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
|
||||
}
|
||||
edges++;
|
||||
}
|
||||
|
||||
junction2 = nodes[way.nodeIds[way.nodeIds.Count - 1]];
|
||||
junction1.edges.Add(new Edge(junction2, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction1.lat, junction1.lon, weight, junction2.lat, junction2.lon);
|
||||
|
||||
if (!way.oneway)
|
||||
{
|
||||
junction2.edges.Add(new Edge(junction1, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction2.lat, junction2.lon, weight, junction1.lat, junction1.lon);
|
||||
edges++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int index = way.nodeIds.Count - 2; index > 1; index--)
|
||||
for (int index = way.nodeIds.Count - 2; index > 0; index--)
|
||||
{
|
||||
Node currentNode = nodes[way.nodeIds[index]];
|
||||
if (count[way.nodeIds[index]] > 1)
|
||||
Node nextNode = nodes[way.nodeIds[index - 1]];
|
||||
weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
|
||||
if (count[way.nodeIds[index - 1]] > 1 || index == 1)
|
||||
{
|
||||
junction2 = nodes[way.nodeIds[index]];
|
||||
/*
|
||||
* If Node is referenced more than once => Junction
|
||||
* If Node is last node of way => Junction
|
||||
* Add an edge between two junctions
|
||||
*/
|
||||
junction2 = nodes[way.nodeIds[index - 1]];
|
||||
junction1.edges.Add(new Edge(junction2, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction1.lat, junction1.lon, weight, junction2.lat, junction2.lon);
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index], weight, way.nodeIds[index - 1]);
|
||||
edges++;
|
||||
|
||||
if (!way.oneway)
|
||||
{
|
||||
junction2.edges.Add(new Edge(junction1, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction2.lat, junction2.lon, weight, junction1.lat, junction1.lon);
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index - 1], weight, way.nodeIds[index]);
|
||||
edges++;
|
||||
}
|
||||
|
||||
junction1 = junction2;
|
||||
weight = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
Node nextNode = nodes[way.nodeIds[index - 1]];
|
||||
weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
|
||||
}
|
||||
edges++;
|
||||
}
|
||||
|
||||
junction2 = nodes[way.nodeIds[way.nodeIds.Count - 1]];
|
||||
junction1.edges.Add(new Edge(junction2, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction1.lat, junction1.lon, weight, junction2.lat, junction2.lon);
|
||||
|
||||
if (!way.oneway)
|
||||
{
|
||||
junction2.edges.Add(new Edge(junction1, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} {1} -- {2} --> {3} {4}", junction2.lat, junction2.lon, weight, junction1.lat, junction1.lon);
|
||||
edges++;
|
||||
}
|
||||
}
|
||||
}
|
||||
reader.Close();
|
||||
GC.Collect();
|
||||
|
||||
|
||||
|
||||
logger?.Log(LogLevel.DEBUG, "Edges: {0}", edges);
|
||||
logger?.Log(LogLevel.DEBUG, "Loaded Edges: {0}", edges);
|
||||
return nodes.Where(node => count[node.Key] > 1).ToDictionary(node => node.Key, node => node.Value);
|
||||
}
|
||||
|
||||
|
@ -17,7 +17,7 @@ namespace astar
|
||||
{
|
||||
Node n1 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
|
||||
Node n2 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
|
||||
logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3}", n1.lat, n1.lon, n2.lat, n2.lon);
|
||||
logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", n1.lat, n1.lon, n2.lat, n2.lon, Utils.DistanceBetweenNodes(n1,n2));
|
||||
path = FindPath(ref nodes, n1, n2, ref logger);
|
||||
}
|
||||
|
||||
@ -41,7 +41,7 @@ namespace astar
|
||||
/*
|
||||
*
|
||||
*/
|
||||
private static List<Node> FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, ref Logger ?logger)
|
||||
private static List<Node> FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, ref Logger? logger)
|
||||
{
|
||||
Reset(ref nodes);
|
||||
List<Node> toVisit = new();
|
||||
@ -49,7 +49,7 @@ namespace astar
|
||||
Node currentNode = start;
|
||||
start.pathLength = 0;
|
||||
start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
|
||||
while(currentNode != goal && toVisit.Count > 0)
|
||||
while (currentNode != goal && toVisit.Count > 0)
|
||||
{
|
||||
currentNode = toVisit.First();
|
||||
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
|
||||
@ -71,7 +71,37 @@ namespace astar
|
||||
|
||||
List<Node> path = new();
|
||||
|
||||
if (currentNode != goal)
|
||||
if (currentNode == goal)
|
||||
{
|
||||
if(toVisit[0].pathLength < goal.pathLength)
|
||||
{
|
||||
logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
|
||||
while (toVisit[0].pathLength < goal.pathLength)
|
||||
{
|
||||
currentNode = toVisit.First();
|
||||
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
|
||||
//Check all neighbors of current node
|
||||
foreach (Edge e in currentNode.edges)
|
||||
{
|
||||
if (e.neighbor.goalDistance == double.MaxValue)
|
||||
e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
|
||||
if (e.neighbor.pathLength > currentNode.pathLength + e.weight)
|
||||
{
|
||||
e.neighbor.pathLength = currentNode.pathLength + e.weight;
|
||||
e.neighbor.previousNode = currentNode;
|
||||
toVisit.Add(e.neighbor);
|
||||
}
|
||||
}
|
||||
toVisit.Remove(currentNode); //"Mark" as visited
|
||||
toVisit.Sort(CompareDistanceToGoal);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon);
|
||||
return path;
|
||||
@ -97,9 +127,9 @@ namespace astar
|
||||
else
|
||||
{
|
||||
if (n1.goalDistance < n2.goalDistance)
|
||||
return 1;
|
||||
else if (n1.goalDistance > n2.goalDistance)
|
||||
return -1;
|
||||
else if (n1.goalDistance > n2.goalDistance)
|
||||
return 1;
|
||||
else return 0;
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user