GitHub
commit
f50961d37c
@ -1,4 +1,19 @@
|
||||
Logging.Logger logger = new (Logging.LogType.CONSOLE, Logging.LogLevel.DEBUG);
|
||||
Dictionary<UInt64, Graph.Node> nodes = OpenStreetMap_Importer.Importer.Import(@"", logger);
|
||||
logger.level = Logging.LogLevel.VERBOSE;
|
||||
astar.Astar astar = new(nodes, logger);
|
||||
using Graph;
|
||||
using Logging;
|
||||
using astar;
|
||||
|
||||
Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG);
|
||||
Dictionary<ulong, Node> graph = OpenStreetMap_Importer.Importer.Import(@"", true, logger);
|
||||
logger.level = LogLevel.DEBUG;
|
||||
|
||||
Random r = new();
|
||||
List<Node> path;
|
||||
Node n1, n2;
|
||||
do
|
||||
{
|
||||
do
|
||||
{
|
||||
n1 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))];
|
||||
n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))];
|
||||
} while (!Astar.FindPath(ref graph, n1, n2, out path, logger));
|
||||
} while (Console.ReadKey().Key.Equals(ConsoleKey.Enter));
|
||||
@ -2,12 +2,21 @@
|
||||
{
|
||||
public struct Edge
|
||||
{
|
||||
public ulong id { get; }
|
||||
public Node neighbor { get; }
|
||||
public double weight { get; }
|
||||
public Edge(Node neighbor, double weight)
|
||||
public float weight { get; }
|
||||
public Edge(Node neighbor, float weight)
|
||||
{
|
||||
this.neighbor = neighbor;
|
||||
this.weight = weight;
|
||||
this.id = 0;
|
||||
}
|
||||
|
||||
public Edge(Node neighbor, float weight, ulong id)
|
||||
{
|
||||
this.neighbor = neighbor;
|
||||
this.weight = weight;
|
||||
this.id = id;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -4,21 +4,21 @@
|
||||
{
|
||||
public float lat { get; }
|
||||
public float lon { get; }
|
||||
public List<Edge> edges { get; }
|
||||
public HashSet<Edge> edges { get; }
|
||||
|
||||
public Node previousNode { get; set; }
|
||||
public double goalDistance { get; set; }
|
||||
public float goalDistance { get; set; }
|
||||
|
||||
public double pathLength { get; set; }
|
||||
public float pathLength { get; set; }
|
||||
|
||||
public Node(float lat, float lon)
|
||||
{
|
||||
this.lat = lat;
|
||||
this.lon = lon;
|
||||
this.edges = new List<Edge>();
|
||||
this.edges = new();
|
||||
this.previousNode = nullnode;
|
||||
this.goalDistance = double.MaxValue;
|
||||
this.pathLength = double.MaxValue;
|
||||
this.goalDistance = float.MaxValue;
|
||||
this.pathLength = float.MaxValue;
|
||||
}
|
||||
public static Node nullnode = new(float.NaN, float.NaN);
|
||||
}
|
||||
|
||||
@ -2,12 +2,12 @@
|
||||
|
||||
public struct Utils
|
||||
{
|
||||
public static double DistanceBetweenNodes(Node n1, Node n2)
|
||||
public static float DistanceBetweenNodes(Node n1, Node n2)
|
||||
{
|
||||
return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
|
||||
}
|
||||
|
||||
public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
|
||||
public static float DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
|
||||
{
|
||||
const int earthRadius = 6371;
|
||||
double differenceLat = DegreesToRadians(lat2 - lat1);
|
||||
@ -19,7 +19,7 @@
|
||||
double a = Math.Sin(differenceLat / 2) * Math.Sin(differenceLat / 2) + Math.Sin(differenceLon / 2) * Math.Sin(differenceLon / 2) * Math.Cos(lat1Rads) * Math.Cos(lat2Rads);
|
||||
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
|
||||
|
||||
return earthRadius * c;
|
||||
return Convert.ToSingle(earthRadius * c);
|
||||
}
|
||||
|
||||
private static double DegreesToRadians(double deg)
|
||||
|
||||
@ -1,4 +1,7 @@
|
||||
using Logging;
|
||||
#pragma warning disable CS8600
|
||||
#pragma warning disable CS8602
|
||||
#pragma warning disable CS8604
|
||||
using Logging;
|
||||
using System.Xml;
|
||||
using Graph;
|
||||
|
||||
@ -7,282 +10,341 @@ namespace OpenStreetMap_Importer
|
||||
public class Importer
|
||||
{
|
||||
|
||||
public static Dictionary<ulong, Node> Import(string filePath = "", Logger ?logger = null)
|
||||
private static XmlReaderSettings readerSettings = new()
|
||||
{
|
||||
List<Way> ways = new();
|
||||
Dictionary<ulong, Node> nodes = new();
|
||||
Stream mapData;
|
||||
XmlReaderSettings readerSettings = new()
|
||||
{
|
||||
IgnoreWhitespace = true,
|
||||
IgnoreComments = true
|
||||
};
|
||||
IgnoreWhitespace = true,
|
||||
IgnoreComments = true
|
||||
};
|
||||
|
||||
bool wayTag = false;
|
||||
Way currentWay = new();
|
||||
Dictionary<ulong, ushort> count = new();
|
||||
public static Dictionary<ulong, Node> Import(string filePath = "", bool onlyJunctions = true, Logger? logger = null)
|
||||
{
|
||||
/*
|
||||
* Count Node occurances when tag is "highway"
|
||||
*/
|
||||
logger?.Log(LogLevel.INFO, "Counting Node-Occurances...");
|
||||
Stream mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map);
|
||||
Dictionary<ulong, ushort> occuranceCount = CountNodeOccurances(mapData, logger);
|
||||
mapData.Close();
|
||||
logger?.Log(LogLevel.DEBUG, "Way Nodes: {0}", occuranceCount.Count);
|
||||
|
||||
/*
|
||||
* First iteration
|
||||
* Import "ways" with a tag "highway"
|
||||
* Count occurances of "nodes" to find junctions
|
||||
* Import Nodes and Edges
|
||||
*/
|
||||
logger?.Log(LogLevel.INFO, "Importing Graph...");
|
||||
mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map);
|
||||
Dictionary<ulong, Node> graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
|
||||
mapData.Close();
|
||||
occuranceCount.Clear();
|
||||
GC.Collect();
|
||||
logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count);
|
||||
|
||||
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);
|
||||
}
|
||||
XmlReader reader = XmlReader.Create(mapData, readerSettings);
|
||||
reader.MoveToContent();
|
||||
return graph;
|
||||
}
|
||||
|
||||
logger?.Log(LogLevel.INFO, "Importing ways and counting nodes...");
|
||||
while (reader.Read())
|
||||
private static Dictionary<ulong, ushort> CountNodeOccurances(Stream mapData, Logger? logger = null)
|
||||
{
|
||||
Dictionary<ulong, ushort> _occurances = new();
|
||||
|
||||
XmlReader _reader = XmlReader.Create(mapData, readerSettings);
|
||||
XmlReader _wayReader;
|
||||
_reader.MoveToContent();
|
||||
|
||||
bool _isHighway;
|
||||
List<ulong> _currentIds = new();
|
||||
|
||||
while (_reader.ReadToFollowing("way"))
|
||||
{
|
||||
if (reader.Name == "way" && reader.IsStartElement())
|
||||
_isHighway = false;
|
||||
_currentIds.Clear();
|
||||
_wayReader = _reader.ReadSubtree();
|
||||
while (_wayReader.Read())
|
||||
{
|
||||
logger?.Log(LogLevel.VERBOSE, "WAY {0} nodes {1}", currentWay.highway.ToString(), currentWay.nodeIds.Count);
|
||||
if (currentWay.highway != Way.highwayType.NONE)
|
||||
if (_reader.Name == "tag" && _reader.GetAttribute("k").Equals("highway"))
|
||||
{
|
||||
ways.Add(currentWay);
|
||||
foreach (ulong id in currentWay.nodeIds)
|
||||
if(count.ContainsKey(id))
|
||||
count[id]++;
|
||||
try
|
||||
{
|
||||
if (!Enum.Parse(typeof(Way.type), _reader.GetAttribute("v"), true).Equals(Way.type.NONE))
|
||||
_isHighway = true;
|
||||
}
|
||||
catch (ArgumentException) { };
|
||||
}
|
||||
else if(_reader.Name == "nd")
|
||||
{
|
||||
try
|
||||
{
|
||||
_currentIds.Add(Convert.ToUInt64(_reader.GetAttribute("ref")));
|
||||
}
|
||||
catch (FormatException) { };
|
||||
}
|
||||
}
|
||||
if (_isHighway)
|
||||
{
|
||||
foreach(ulong _id in _currentIds)
|
||||
{
|
||||
if (!_occurances.TryAdd(_id, 1))
|
||||
_occurances[_id]++;
|
||||
}
|
||||
}
|
||||
_wayReader.Close();
|
||||
}
|
||||
_reader.Close();
|
||||
GC.Collect();
|
||||
|
||||
return _occurances;
|
||||
}
|
||||
|
||||
private static Dictionary<ulong, Node> CreateGraph(Stream mapData, Dictionary<ulong, ushort> occuranceCount, bool onlyJunctions, Logger? logger = null)
|
||||
{
|
||||
Dictionary<ulong, Node> _tempAll = new();
|
||||
Dictionary<ulong, Node> _graph = new();
|
||||
Way _currentWay;
|
||||
Node _n1, _n2, _currentNode;
|
||||
float _weight, _distance = 0;
|
||||
|
||||
XmlReader _reader = XmlReader.Create(mapData, readerSettings);
|
||||
XmlReader _wayReader;
|
||||
_reader.MoveToContent();
|
||||
|
||||
while (_reader.Read())
|
||||
{
|
||||
if(_reader.Name == "node")
|
||||
{
|
||||
ulong id = Convert.ToUInt64(_reader.GetAttribute("id"));
|
||||
if (occuranceCount.ContainsKey(id))
|
||||
{
|
||||
float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ','));
|
||||
float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ','));
|
||||
_tempAll.Add(id, new Node(lat, lon));
|
||||
logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]);
|
||||
}
|
||||
}
|
||||
else if(_reader.Name == "way")
|
||||
{
|
||||
_wayReader = _reader.ReadSubtree();
|
||||
_currentWay = new();
|
||||
while (_wayReader.Read())
|
||||
{
|
||||
_wayReader = _reader.ReadSubtree();
|
||||
_currentWay.AddTag("id", _reader.GetAttribute("id"));
|
||||
while (_wayReader.Read())
|
||||
{
|
||||
if (_reader.Name == "tag")
|
||||
{
|
||||
string _value = _reader.GetAttribute("v");
|
||||
string _key = _reader.GetAttribute("k");
|
||||
logger?.Log(LogLevel.VERBOSE, "TAG {0} {1}", _key, _value);
|
||||
_currentWay.AddTag(_key, _value);
|
||||
}
|
||||
else if (_reader.Name == "nd")
|
||||
{
|
||||
ulong _id = Convert.ToUInt64(_reader.GetAttribute("ref"));
|
||||
_currentWay.nodeIds.Add(_id);
|
||||
logger?.Log(LogLevel.VERBOSE, "nd: {0}", _id);
|
||||
}
|
||||
}
|
||||
}
|
||||
_wayReader.Close();
|
||||
|
||||
if (!_currentWay.GetHighwayType().Equals(Way.type.NONE))
|
||||
{
|
||||
logger?.Log(LogLevel.VERBOSE, "WAY Nodes: {0} Type: {1}", _currentWay.nodeIds.Count, _currentWay.GetHighwayType());
|
||||
if (!onlyJunctions)
|
||||
{
|
||||
for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
|
||||
{
|
||||
_n1 = _tempAll[_currentWay.nodeIds[_nodeIdIndex]];
|
||||
_n2 = _tempAll[_currentWay.nodeIds[_nodeIdIndex + 1]];
|
||||
_weight = Utils.DistanceBetweenNodes(_n1, _n2) * 1000 / _currentWay.GetMaxSpeed();
|
||||
if (!_currentWay.IsOneWay())
|
||||
{
|
||||
_n1.edges.Add(new Edge(_n2, _weight));
|
||||
_n2.edges.Add(new Edge(_n1, _weight));
|
||||
}
|
||||
else if (_currentWay.IsForward())
|
||||
{
|
||||
_n1.edges.Add(new Edge(_n2, _weight));
|
||||
}
|
||||
else
|
||||
{
|
||||
_n2.edges.Add(new Edge(_n1, _weight));
|
||||
}
|
||||
}
|
||||
_graph = _tempAll;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!_tempAll.TryGetValue(_currentWay.nodeIds[0], out _n1))
|
||||
{
|
||||
_n1 = _graph[_currentWay.nodeIds[0]];
|
||||
}
|
||||
else
|
||||
count.TryAdd(id, 1);
|
||||
}
|
||||
wayTag = true;
|
||||
currentWay = new Way();
|
||||
}
|
||||
else if (reader.Name == "tag" && wayTag)
|
||||
{
|
||||
#pragma warning disable CS8600 //tags will always have a value and key
|
||||
#pragma warning disable CS8604
|
||||
string value = reader.GetAttribute("v");
|
||||
string key = reader.GetAttribute("k");
|
||||
logger?.Log(LogLevel.VERBOSE, "TAG {0} {1}", key, value);
|
||||
#pragma warning restore CS8600
|
||||
switch (key)
|
||||
{
|
||||
case "highway":
|
||||
currentWay.SetHighwayType(value);
|
||||
break;
|
||||
case "oneway":
|
||||
switch (value)
|
||||
{
|
||||
case "yes":
|
||||
currentWay.oneway = true;
|
||||
break;
|
||||
/*case "no":
|
||||
currentWay.oneway = false;
|
||||
break;*/
|
||||
case "-1":
|
||||
currentWay.oneway = true;
|
||||
currentWay.direction = Way.wayDirection.backward;
|
||||
break;
|
||||
_graph.TryAdd(_currentWay.nodeIds[0], _n1);
|
||||
_tempAll.Remove(_currentWay.nodeIds[0]);
|
||||
}
|
||||
break;
|
||||
/*case "name":
|
||||
|
||||
break;*/
|
||||
}
|
||||
#pragma warning restore CS8604
|
||||
}
|
||||
else if(reader.Name == "nd" && wayTag)
|
||||
{
|
||||
ulong id = Convert.ToUInt64(reader.GetAttribute("ref"));
|
||||
currentWay.nodeIds.Add(id);
|
||||
logger?.Log(LogLevel.VERBOSE, "nd: {0}", id);
|
||||
}
|
||||
else if(reader.Name == "node")
|
||||
{
|
||||
wayTag = false;
|
||||
}
|
||||
}
|
||||
|
||||
logger?.Log(LogLevel.DEBUG, "Loaded Ways: {0} Required Nodes: {1}", ways.Count, count.Count);
|
||||
|
||||
reader.Close();
|
||||
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")
|
||||
{
|
||||
ulong id = Convert.ToUInt64(reader.GetAttribute("id"));
|
||||
if (count.ContainsKey(id))
|
||||
{
|
||||
#pragma warning disable CS8602 //node will always have a lat and lon
|
||||
float lat = Convert.ToSingle(reader.GetAttribute("lat").Replace('.', ','));
|
||||
float lon = Convert.ToSingle(reader.GetAttribute("lon").Replace('.', ','));
|
||||
#pragma warning restore CS8602
|
||||
nodes.TryAdd(id, new Node(lat, lon));
|
||||
logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, count[id]);
|
||||
}
|
||||
}
|
||||
}
|
||||
/*
|
||||
* 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 = 0; index < way.nodeIds.Count - 1; index++)
|
||||
{
|
||||
Node currentNode = nodes[way.nodeIds[index]];
|
||||
Node nextNode = nodes[way.nodeIds[index + 1]];
|
||||
weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
|
||||
if (count[way.nodeIds[index + 1]] > 1 || index == way.nodeIds.Count - 2)
|
||||
{
|
||||
/*
|
||||
* 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}", way.nodeIds[index], weight, way.nodeIds[index + 1]);
|
||||
edges++;
|
||||
|
||||
if (!way.oneway)
|
||||
_currentNode = _n1;
|
||||
for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
|
||||
{
|
||||
junction2.edges.Add(new Edge(junction1, weight));
|
||||
logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index + 1], weight, way.nodeIds[index]);
|
||||
edges++;
|
||||
if (!_tempAll.TryGetValue(_currentWay.nodeIds[_nodeIdIndex + 1], out _n2))
|
||||
_n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]];
|
||||
_distance += Utils.DistanceBetweenNodes(_currentNode, _n2);
|
||||
if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found
|
||||
{
|
||||
_weight = _distance * 1000 / _currentWay.GetMaxSpeed();
|
||||
_distance = 0;
|
||||
_graph.TryAdd(_currentWay.nodeIds[_nodeIdIndex + 1], _n2);
|
||||
if (!_currentWay.IsOneWay())
|
||||
{
|
||||
_n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId()));
|
||||
_n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId()));
|
||||
}
|
||||
else if (_currentWay.IsForward())
|
||||
{
|
||||
_n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId()));
|
||||
}
|
||||
else
|
||||
{
|
||||
_n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId()));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_tempAll.Remove(_currentWay.nodeIds[_nodeIdIndex]);
|
||||
}
|
||||
_currentNode = _n2;
|
||||
}
|
||||
|
||||
junction1 = junction2;
|
||||
weight = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int index = way.nodeIds.Count - 2; index > 0; index--)
|
||||
{
|
||||
Node currentNode = nodes[way.nodeIds[index]];
|
||||
Node nextNode = nodes[way.nodeIds[index - 1]];
|
||||
weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
|
||||
if (count[way.nodeIds[index - 1]] > 1 || index == 1)
|
||||
{
|
||||
/*
|
||||
* 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}", 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}", way.nodeIds[index - 1], weight, way.nodeIds[index]);
|
||||
edges++;
|
||||
}
|
||||
|
||||
junction1 = junction2;
|
||||
weight = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
reader.Close();
|
||||
_reader.Close();
|
||||
GC.Collect();
|
||||
|
||||
logger?.Log(LogLevel.DEBUG, "Loaded Edges: {0}", edges);
|
||||
return nodes.Where(node => count[node.Key] > 1).ToDictionary(node => node.Key, node => node.Value);
|
||||
return _graph;
|
||||
}
|
||||
|
||||
internal struct Way
|
||||
{
|
||||
public List<ulong> nodeIds;
|
||||
public bool oneway;
|
||||
public wayDirection direction;
|
||||
public highwayType highway;
|
||||
public enum wayDirection { forward, backward }
|
||||
public enum highwayType : uint
|
||||
{
|
||||
NONE = 1,
|
||||
motorway = 130,
|
||||
trunk = 125,
|
||||
primary = 110,
|
||||
secondary = 100,
|
||||
tertiary = 80,
|
||||
unclassified = 40,
|
||||
residential = 23,
|
||||
motorway_link = 55,
|
||||
trunk_link = 50,
|
||||
primary_link = 30,
|
||||
secondary_link = 25,
|
||||
tertiary_link = 24,
|
||||
living_street = 20,
|
||||
service = 14,
|
||||
pedestrian = 12,
|
||||
track = 6,
|
||||
bus_guideway = 15,
|
||||
escape = 3,
|
||||
raceway = 4,
|
||||
road = 28,
|
||||
busway = 13,
|
||||
footway = 8,
|
||||
bridleway = 7,
|
||||
steps = 5,
|
||||
corridor = 9,
|
||||
path = 10,
|
||||
cycleway = 11,
|
||||
construction = 2
|
||||
}
|
||||
private Dictionary<string, object> tags;
|
||||
|
||||
|
||||
public Dictionary<type, int> speed = new() {
|
||||
{ type.NONE, 1 },
|
||||
{ type.motorway, 130 },
|
||||
{ type.trunk, 125 },
|
||||
{ type.primary, 110 },
|
||||
{ type.secondary, 100 },
|
||||
{ type.tertiary, 90 },
|
||||
{ type.unclassified, 40 },
|
||||
{ type.residential, 20 },
|
||||
{ type.motorway_link, 50 },
|
||||
{ type.trunk_link, 50 },
|
||||
{ type.primary_link, 30 },
|
||||
{ type.secondary_link, 25 },
|
||||
{ type.tertiary_link, 25 },
|
||||
{ type.living_street, 20 },
|
||||
{ type.service, 10 },
|
||||
{ type.pedestrian, 10 },
|
||||
{ type.track, 1 },
|
||||
{ type.bus_guideway, 5 },
|
||||
{ type.escape, 1 },
|
||||
{ type.raceway, 1 },
|
||||
{ type.road, 25 },
|
||||
{ type.busway, 5 },
|
||||
{ type.footway, 1 },
|
||||
{ type.bridleway, 1 },
|
||||
{ type.steps, 1 },
|
||||
{ type.corridor, 1 },
|
||||
{ type.path, 10 },
|
||||
{ type.cycleway, 5 },
|
||||
{ type.construction, 1 }
|
||||
};
|
||||
public enum type { NONE, motorway, trunk, primary, secondary, tertiary, unclassified, residential, motorway_link, trunk_link, primary_link, secondary_link, tertiary_link, living_street, service, pedestrian, track, bus_guideway, escape, raceway, road, busway, footway, bridleway, steps, corridor, path, cycleway, construction }
|
||||
|
||||
|
||||
public Way()
|
||||
{
|
||||
this.nodeIds = new List<ulong>();
|
||||
this.oneway = false;
|
||||
this.direction = wayDirection.forward;
|
||||
this.highway = highwayType.NONE;
|
||||
this.tags = new();
|
||||
}
|
||||
public void AddTag(string key, string value, Logger? logger = null)
|
||||
{
|
||||
switch (key)
|
||||
{
|
||||
case "highway":
|
||||
try
|
||||
{
|
||||
this.tags.Add(key, (type)Enum.Parse(typeof(type), value, true));
|
||||
if (this.GetMaxSpeed().Equals((int)type.NONE))
|
||||
{
|
||||
this.tags["maxspeed"] = (int)this.GetHighwayType();
|
||||
}
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
this.tags.Add(key, type.NONE);
|
||||
}
|
||||
break;
|
||||
case "maxspeed":
|
||||
try
|
||||
{
|
||||
if (this.tags.ContainsKey("maxspeed"))
|
||||
this.tags["maxspeed"] = Convert.ToInt32(value);
|
||||
else
|
||||
this.tags.Add(key, Convert.ToInt32(value));
|
||||
}
|
||||
catch (FormatException)
|
||||
{
|
||||
this.tags.Add(key, (int)this.GetHighwayType());
|
||||
}
|
||||
break;
|
||||
case "oneway":
|
||||
switch (value)
|
||||
{
|
||||
case "yes":
|
||||
this.tags.Add(key, true);
|
||||
break;
|
||||
case "-1":
|
||||
this.tags.Add("forward", false);
|
||||
break;
|
||||
case "no":
|
||||
this.tags.Add(key, false);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case "id":
|
||||
this.tags.Add(key, Convert.ToUInt64(value));
|
||||
break;
|
||||
default:
|
||||
logger?.Log(LogLevel.VERBOSE, "Tag {0} - {1} was not added.", key, value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public void SetHighwayType(string waytype)
|
||||
public ulong GetId()
|
||||
{
|
||||
try
|
||||
{
|
||||
this.highway = (highwayType)Enum.Parse(typeof(highwayType), waytype, true);
|
||||
}catch(Exception)
|
||||
{
|
||||
this.highway = highwayType.NONE;
|
||||
}
|
||||
return this.tags.ContainsKey("id") ? (ulong)this.tags["id"] : 0;
|
||||
}
|
||||
|
||||
public type GetHighwayType()
|
||||
{
|
||||
return this.tags.ContainsKey("highway") ? (type)this.tags["highway"] : type.NONE;
|
||||
}
|
||||
|
||||
public bool IsOneWay()
|
||||
{
|
||||
return this.tags.ContainsKey("oneway") ? (bool)this.tags["oneway"] : false;
|
||||
}
|
||||
|
||||
public int GetMaxSpeed()
|
||||
{
|
||||
return this.tags.ContainsKey("maxspeed") ? (int)this.tags["maxspeed"] : (int)this.GetHighwayType();
|
||||
|
||||
}
|
||||
|
||||
public bool IsForward()
|
||||
{
|
||||
return this.tags.ContainsKey("forward") ? (bool)this.tags["forward"] : true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
# astar
|
||||
### astar
|
||||
Test of A* Algorithm
|
||||
|
||||
#Project
|
||||
# Project
|
||||
|
||||
- *astar* and *Graph* can be used as standalone (combined)
|
||||
- *OpenStreetMap Importer* Loads a .osm file (currently as ressource included). Requires *Graph*.
|
||||
|
||||
120
astar/Astar.cs
120
astar/Astar.cs
@ -6,26 +6,6 @@ namespace astar
|
||||
public class Astar
|
||||
{
|
||||
|
||||
/*
|
||||
* Loads the graph, chooses two nodes at random and calls a*
|
||||
*/
|
||||
public Astar(Dictionary<UInt64, Node> nodes, Logger ?logger = null)
|
||||
{
|
||||
Random r = new();
|
||||
List<Node> path = new();
|
||||
while(path.Count < 1)
|
||||
{
|
||||
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} Distance {4}", n1.lat, n1.lon, n2.lat, n2.lon, Utils.DistanceBetweenNodes(n1,n2));
|
||||
path = FindPath(ref nodes, n1, n2, ref logger);
|
||||
}
|
||||
|
||||
logger?.Log(LogLevel.INFO, "Path found");
|
||||
foreach (Node n in path)
|
||||
logger?.Log(LogLevel.INFO, "lat {0:000.00000} lon {1:000.00000} traveled {2:0000.00} / {3:0000.00} beeline {4:0000.00}", n.lat, n.lon, n.pathLength, path.ElementAt(path.Count-1).pathLength, n.goalDistance);
|
||||
}
|
||||
|
||||
/*
|
||||
* Resets the calculated previous nodes and distance to goal
|
||||
*/
|
||||
@ -34,77 +14,55 @@ namespace astar
|
||||
foreach(Node n in nodes.Values)
|
||||
{
|
||||
n.previousNode = Node.nullnode;
|
||||
n.goalDistance = double.MaxValue;
|
||||
n.goalDistance = float.MaxValue;
|
||||
n.pathLength = float.MaxValue;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
private static List<Node> FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, ref Logger? logger)
|
||||
public static bool FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, out List<Node> path, Logger? logger)
|
||||
{
|
||||
path = new List<Node>();
|
||||
logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal));
|
||||
Reset(ref nodes);
|
||||
List<Node> toVisit = new();
|
||||
toVisit.Add(start);
|
||||
Node currentNode = start;
|
||||
start.pathLength = 0;
|
||||
start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
|
||||
while (currentNode != goal && toVisit.Count > 0)
|
||||
while (toVisit.Count > 0 && toVisit[0].pathLength < goal.pathLength)
|
||||
{
|
||||
if(currentNode == goal)
|
||||
{
|
||||
logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
|
||||
}
|
||||
currentNode = toVisit.First();
|
||||
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
|
||||
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {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.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
|
||||
e.neighbor.pathLength = currentNode.pathLength + e.weight;
|
||||
e.neighbor.previousNode = currentNode;
|
||||
toVisit.Add(e.neighbor);
|
||||
}
|
||||
}
|
||||
|
||||
toVisit.Remove(currentNode); //"Mark" as visited
|
||||
toVisit.Sort(CompareDistanceToGoal);
|
||||
toVisit.Sort(CompareDistance);
|
||||
}
|
||||
|
||||
List<Node> path = new();
|
||||
|
||||
if (currentNode == goal)
|
||||
if(goal.previousNode != Node.nullnode)
|
||||
{
|
||||
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.");
|
||||
}
|
||||
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;
|
||||
return false;
|
||||
}
|
||||
|
||||
path.Add(goal);
|
||||
@ -114,13 +72,33 @@ namespace astar
|
||||
currentNode = currentNode.previousNode;
|
||||
}
|
||||
path.Reverse();
|
||||
return path;
|
||||
|
||||
logger?.Log(LogLevel.INFO, "Path found");
|
||||
float distance = 0;
|
||||
Node prev = Node.nullnode;
|
||||
TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).pathLength);
|
||||
|
||||
foreach (Node n in path)
|
||||
{
|
||||
if(!prev.Equals(Node.nullnode))
|
||||
{
|
||||
distance += Utils.DistanceBetweenNodes(prev, n);
|
||||
}
|
||||
prev = n;
|
||||
logger?.Log(LogLevel.DEBUG, "lat {0:000.00000} lon {1:000.00000} traveled {5:0000.00}km in {2:G} / {3:G} Great-Circle to Goal {4:0000.00}", n.lat, n.lon, TimeSpan.FromSeconds(n.pathLength), totalTime, n.goalDistance, distance);
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Compares two nodes and returns the node closer to the goal
|
||||
* -1 => n1 smaller n2
|
||||
* 0 => n1 equal n2
|
||||
* 1 => n1 larger n2
|
||||
*/
|
||||
private static int CompareDistanceToGoal(Node n1, Node n2)
|
||||
private static int CompareDistance(Node n1, Node n2)
|
||||
{
|
||||
if (n1 == null || n2 == null)
|
||||
return 0;
|
||||
@ -133,5 +111,25 @@ namespace astar
|
||||
else return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Compares two nodes and returns the node with the shorter path
|
||||
* -1 => n1 smaller n2
|
||||
* 0 => n1 equal n2
|
||||
* 1 => n1 larger n2
|
||||
*/
|
||||
private static int ComparePathLength(Node n1, Node n2)
|
||||
{
|
||||
if (n1 == null || n2 == null)
|
||||
return 0;
|
||||
else
|
||||
{
|
||||
if (n1.pathLength < n2.pathLength)
|
||||
return -1;
|
||||
else if (n1.pathLength > n2.pathLength)
|
||||
return 1;
|
||||
else return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user