glax/AStar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial change

Browse Source
This commit is contained in:
C9Glax 2022-11-13 14:02:27 +01:00
parent 23bab6a593
commit a4ebf05d7a
7 changed files with 142 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Executable/Program.cs
View File

@ -3,7 +3,7 @@ using Logging;
using astar;
Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG);
Dictionary<ulong, Node> graph = OpenStreetMap_Importer.Importer.Import(@"", true, logger);
Graph.Graph graph = OpenStreetMap_Importer.Importer.Import(@"", true, logger);
logger.level = LogLevel.DEBUG;
Random r = new();
@ -13,8 +13,8 @@ do
{
do
{
n1 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))];
n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))];
_route = Astar.FindPath(ref graph, n1, n2, logger);
n1 = graph.nodes[r.Next(0, graph.nodes.Count - 1)];
n2 = graph.nodes[r.Next(0, graph.nodes.Count - 1)];
_route = new Astar().FindPath(graph, n1, n2, logger);
} while (!_route.routeFound);
} while (Console.ReadKey().Key.Equals(ConsoleKey.Enter));

73
Graph/Graph.cs Normal file
View File

@ -0,0 +1,73 @@

namespace Graph
{
public class Graph
{
public List<Node> nodes { get; }
public Graph()
{
this.nodes = new();
}
public bool AddNode(Node n)
{
if (!this.ContainsNode(n.id))
{
this.nodes.Add(n);
return true;
}
else
{
return false;
}
}
public Node? GetNode(ulong id)
{
foreach(Node n in this.nodes)
{
if (n.id == id)
return n;
}
return null;
}
public bool ContainsNode(ulong id)
{
return this.GetNode(id) != null;
}
public bool RemoveNode(ulong id)
{
Node? n = this.GetNode(id);
if(n != null)
{
this.nodes.Remove(n);
return true;
}
else
{
return false;
}
}
public bool RemoveNode(Node n)
{
if (this.RemoveNode(n.id))
{
this.nodes.Remove(n);
return true;
}
else
{
return false;
}
}
public Node ClosestNodeToCoordinates(float lat, float lon)
{
throw new NotImplementedException();
}
}
}

13
Graph/Node.cs
View File

@ -4,21 +4,16 @@
{
public float lat { get; }
public float lon { get; }
public ulong id { get; }
public HashSet<Edge> edges { get; }
public Node? previousNode { get; set; }
public float goalDistance { get; set; }
public float timeRequired { get; set; }
public Node(float lat, float lon)
public Node(ulong id, float lat, float lon)
{
this.id = id;
this.lat = lat;
this.lon = lon;
this.edges = new();
this.previousNode = null;
this.goalDistance = float.MaxValue;
this.timeRequired = float.MaxValue;
}
public Edge? GetEdgeToNode(Node n)

3
Graph/Utils.cs
View File

@ -1,6 +1,5 @@
namespace Graph
namespace Graph.Utils
{
public struct Utils
{
public static double DistanceBetweenNodes(Node n1, Node n2)

25
OpenStreetMap Importer/Importer.cs
View File

@ -4,6 +4,7 @@
using Logging;
using System.Xml;
using Graph;
using Graph.Utils;
namespace OpenStreetMap_Importer
{
@ -16,7 +17,7 @@ namespace OpenStreetMap_Importer
IgnoreComments = true
};
public static Dictionary<ulong, Node> Import(string filePath = "", bool onlyJunctions = true, Logger? logger = null)
public static Graph.Graph Import(string filePath = "", bool onlyJunctions = true, Logger? logger = null)
{
/*
* Count Node occurances when tag is "highway"
@ -32,14 +33,14 @@ namespace OpenStreetMap_Importer
*/
mapData.Position = 0;
logger?.Log(LogLevel.INFO, "Importing Graph...");
Dictionary<ulong, Node> graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count);
Graph.Graph _graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", _graph.nodes.Count);
mapData.Close();
occuranceCount.Clear();
GC.Collect();
return graph;
return _graph;
}
private static Dictionary<ulong, ushort> CountNodeOccurances(Stream mapData, Logger? logger = null)
@ -97,9 +98,9 @@ namespace OpenStreetMap_Importer
return _occurances;
}
private static Dictionary<ulong, Node> CreateGraph(Stream mapData, Dictionary<ulong, ushort> occuranceCount, bool onlyJunctions, Logger? logger = null)
private static Graph.Graph CreateGraph(Stream mapData, Dictionary<ulong, ushort> occuranceCount, bool onlyJunctions, Logger? logger = null)
{
Dictionary<ulong, Node> _graph = new();
Graph.Graph _graph = new();
Way _currentWay;
Node _n1, _n2, _currentNode;
float _time, _distance = 0;
@ -117,7 +118,7 @@ namespace OpenStreetMap_Importer
{
float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ','));
float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ','));
_graph.Add(id, new Node(lat, lon));
_graph.AddNode(new Node(id, lat, lon));
logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]);
}
}
@ -156,8 +157,8 @@ namespace OpenStreetMap_Importer
{
for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
{
_n1 = _graph[_currentWay.nodeIds[_nodeIdIndex]];
_n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]];
_n1 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex]);
_n2 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex + 1]);
_distance = Convert.ToSingle(Utils.DistanceBetweenNodes(_n1, _n2));
_time = _distance / _currentWay.GetMaxSpeed();
@ -179,11 +180,11 @@ namespace OpenStreetMap_Importer
}
else
{
_n1 = _graph[_currentWay.nodeIds[0]];
_n1 = _graph.GetNode(_currentWay.nodeIds[0]);
_currentNode = _n1;
for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
{
_n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]];
_n2 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex + 1]);
_distance += Convert.ToSingle(Utils.DistanceBetweenNodes(_currentNode, _n2));
if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found
{
@ -206,7 +207,7 @@ namespace OpenStreetMap_Importer
}
else
{
_graph.Remove(_currentWay.nodeIds[_nodeIdIndex]); //Not a junction
_graph.RemoveNode(_currentWay.nodeIds[_nodeIdIndex]); //Not a junction
}
_currentNode = _n2;
}

74
astar/Astar.cs
View File

@ -1,51 +1,39 @@
using Logging;
using Graph;
using Graph.Utils;
namespace astar
{
public class Astar
{
Dictionary<Node, float> timeRequired = new();
Dictionary<Node, float> goalDistance = new();
Dictionary<Node, Node> previousNode = new();
/*
* Resets the calculated previous nodes and distance to goal
*/
private static void Reset(ref Dictionary<ulong, Node> nodes)
public Route FindPath(Graph.Graph graph, Node start, Node goal, Logger? logger)
{
foreach(Node n in nodes.Values)
{
n.previousNode = null;
n.goalDistance = float.MaxValue;
n.timeRequired = float.MaxValue;
}
}
public static Route FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, Logger? logger)
{
Route _route = new Route();
logger?.Log(LogLevel.INFO, "From {0:000.00000}#{1:000.00000} to {2:000.00000}#{3:000.00000} Great-Circle {4:00000.00}km", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal)/1000);
Reset(ref nodes);
List<Node> toVisit = new();
toVisit.Add(start);
Node currentNode = start;
start.timeRequired = 0;
start.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal));
while (toVisit.Count > 0 && toVisit[0].timeRequired < goal.timeRequired)
timeRequired.Add(start, 0);
goalDistance.Add(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)));
while (toVisit.Count > 0 && timeRequired[toVisit[0]] < timeRequired[goal])
{
if(currentNode == goal)
{
logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
}
currentNode = toVisit.First();
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.timeRequired, currentNode.goalDistance);
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, timeRequired[currentNode], goalDistance[currentNode]);
//Check all neighbors of current node
foreach (Edge e in currentNode.edges)
{
if (e.neighbor.timeRequired > currentNode.timeRequired + e.time)
if (timeRequired[e.neighbor] > timeRequired[currentNode] + e.time)
{
e.neighbor.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal));
e.neighbor.timeRequired = currentNode.timeRequired + e.time;
e.neighbor.previousNode = currentNode;
goalDistance[e.neighbor] = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal));
timeRequired[e.neighbor] = timeRequired[currentNode] + e.time;
previousNode[e.neighbor] = currentNode;
toVisit.Add(e.neighbor);
}
}
@ -53,18 +41,16 @@ namespace astar
toVisit.Remove(currentNode); //"Mark" as visited
toVisit.Sort(CompareDistance);
}
if(goal.previousNode != null)
if(previousNode[goal] != null)
{
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
currentNode = goal;
_route.routeFound = true;
_route.time = goal.timeRequired;
}
else
{
logger?.Log(LogLevel.INFO, "No path between {0:000.00000}#{1:000.00000} and {2:000.00000}#{3:000.00000}", start.lat, start.lon, goal.lat, goal.lon);
_route.routeFound = false;
return _route;
return new Route(new List<Step>(), false, float.MaxValue, float.MaxValue);
}
List<Node> tempNodes = new();
@ -72,20 +58,26 @@ namespace astar
while(currentNode != start)
{
#pragma warning disable CS8604 // Route was found, so has to have a previous node
tempNodes.Add(currentNode.previousNode);
tempNodes.Add(previousNode[currentNode]);
#pragma warning restore CS8604
currentNode = currentNode.previousNode;
currentNode = previousNode[currentNode];
}
tempNodes.Reverse();
List<Step> steps = new();
float totalDistance = 0;
for(int i = 0; i < tempNodes.Count - 1; i++)
{
#pragma warning disable CS8600, CS8604 // Route was found, so has to have an edge
Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]);
_route.AddStep(tempNodes[i], e);
steps.Add(new Step(tempNodes[i], e, timeRequired[tempNodes[i]], goalDistance[tempNodes[i]]));
#pragma warning restore CS8600, CS8604
_route.distance += e.distance;
totalDistance += e.distance;
}
Route _route = new Route(steps, true, totalDistance, timeRequired[goal]);
logger?.Log(LogLevel.INFO, "Path found");
if(logger?.level > LogLevel.INFO)
@ -100,7 +92,7 @@ namespace astar
Step s = _route.steps[i];
time += s.edge.time;
distance += s.edge.distance;
logger?.Log(LogLevel.DEBUG, "Step {0:000} From {1:000.00000}#{2:000.00000} To {3:000.00000}#{4:000.00000} along {5:0000000000} after {6} and {7:0000.00}km", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(s.start.timeRequired), distance/1000);
logger?.Log(LogLevel.DEBUG, "Step {0:000} From {1:000.00000}#{2:000.00000} To {3:000.00000}#{4:000.00000} along {5:0000000000} after {6} and {7:0000.00}km", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(timeRequired[s.start]), distance/1000);
}
}
@ -113,15 +105,15 @@ namespace astar
* 0 => n1 equal n2
* 1 => n1 larger n2
*/
private static int CompareDistance(Node n1, Node n2)
private int CompareDistance(Node n1, Node n2)
{
if (n1 == null || n2 == null)
return 0;
else
{
if (n1.goalDistance < n2.goalDistance)
if (goalDistance[n1] < goalDistance[n2])
return -1;
else if (n1.goalDistance > n2.goalDistance)
else if (goalDistance[n1] > goalDistance[n2])
return 1;
else return 0;
}
@ -133,15 +125,15 @@ namespace astar
* 0 => n1 equal n2
* 1 => n1 larger n2
*/
private static int ComparePathLength(Node n1, Node n2)
private int ComparePathLength(Node n1, Node n2)
{
if (n1 == null || n2 == null)
return 0;
else
{
if (n1.timeRequired < n2.timeRequired)
if (timeRequired[n1] < timeRequired[n2])
return -1;
else if (n1.timeRequired > n2.timeRequired)
else if (timeRequired[n1] > timeRequired[n2])
return 1;
else return 0;
}

25
astar/Route.cs
View File

@ -4,19 +4,17 @@ namespace astar
public class Route
{
public List<Step> steps { get; }
public bool routeFound { get; set; }
public float distance { get; set; }
public float time { get; set; }
public bool routeFound { get; }
public float distance { get; }
public float time { get; }
public Route()
{
this.steps = new();
this.distance = 0;
}
public void AddStep(Node start, Edge way)
public Route(List<Step> steps, bool routeFound, float distance, float timeRequired)
{
this.steps.Add(new Step(start, way));
this.steps = steps;
this.routeFound = routeFound;
this.distance = distance;
this.time = timeRequired;
}
}
@ -24,10 +22,15 @@ namespace astar
{
public Node start { get; }
public Edge edge { get; }
public Step(Node start, Edge route)
public float timeRequired { get; }
public float goalDistance { get; }
public Step(Node start, Edge route, float timeRequired, float goalDistance)
{
this.start = start;
this.edge = route;
this.timeRequired = timeRequired;
this.goalDistance = goalDistance;
}
}
}