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

Merge pull request #9 from C9Glax/dev

Browse Source 
Merge before Project Split
This commit is contained in:
Glax 2022-11-13 16:10:38 +01:00 committed by GitHub
commit 903958a22a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 279 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Executable/Program.cs
View File

@ -3,17 +3,19 @@ using Logging;
using astar; using astar;
Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG); Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG);
Dictionary<ulong, Node> graph = OpenStreetMap_Importer.Importer.Import(@"", true, logger); Graph.Graph graph = OpenStreetMap_Importer.Importer.Import(@"C:\Users\glax\Downloads\oberbayern-latest.osm", true, logger);
logger.level = LogLevel.DEBUG; logger.level = LogLevel.DEBUG;
Random r = new(); Random r = new();
List<Node> path; Route _route;
Node n1, n2; Node n1, n2;
do do
{ {
do do
{ {
n1 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; n1 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1));
n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; n2 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1));
} while (!Astar.FindPath(ref graph, n1, n2, out path, logger)); _route = new Astar().FindPath(graph, n1, n2, logger);
} while (!_route.routeFound);
logger.Log(LogLevel.INFO, "Press Enter to find new path.");
} while (Console.ReadKey().Key.Equals(ConsoleKey.Enter)); } while (Console.ReadKey().Key.Equals(ConsoleKey.Enter));

18
Graph/Edge.cs
View File

@ -1,22 +1,18 @@
namespace Graph namespace Graph
{ {
public struct Edge public class Edge
{ {
public ulong id { get; } public ulong id { get; }
public Node neighbor { get; } public Node neighbor { get; }
public float weight { get; } public float time { get; }
public Edge(Node neighbor, float weight) public float distance { get; }
{
this.neighbor = neighbor;
this.weight = weight;
this.id = 0;
}
public Edge(Node neighbor, float weight, ulong id) public Edge(Node neighbor, float time, float distance, ulong? id)
{ {
this.neighbor = neighbor; this.neighbor = neighbor;
this.weight = weight; this.time = time;
this.id = id; this.distance = distance;
this.id = (id != null) ? id.Value : 0;
} }
} }
} }

61
Graph/Graph.cs Normal file
View File

@ -0,0 +1,61 @@

namespace Graph
{
public class Graph
{
private Dictionary<ulong, Node> nodes { get; }
public Graph()
{
this.nodes = new();
}
public bool AddNode(ulong id, Node n)
{
return this.nodes.TryAdd(id, n);
}
public Node NodeAtIndex(int i)
{
return this.nodes.Values.ToArray()[i];
}
public int GetNodeCount()
{
return this.nodes.Count;
}
public Node? GetNode(ulong id)
{
if (this.nodes.TryGetValue(id, out Node? n))
return n;
else
return null;
}
public bool ContainsNode(ulong id)
{
return this.nodes.ContainsKey(id);
}
public bool ContainsNode(Node n)
{
return this.nodes.Values.Contains(n);
}
public bool RemoveNode(ulong id)
{
return this.nodes.Remove(id);
}
public bool RemoveNode(Node n)
{
throw new NotImplementedException();
}
public Node ClosestNodeToCoordinates(float lat, float lon)
{
throw new NotImplementedException();
}
}
}

18
Graph/Node.cs
View File

@ -4,22 +4,22 @@
{ {
public float lat { get; } public float lat { get; }
public float lon { get; } public float lon { get; }
public HashSet<Edge> edges { get; } public HashSet<Edge> edges { get; }
public Node previousNode { get; set; }
public float goalDistance { get; set; }
public float pathLength { get; set; }
public Node(float lat, float lon) public Node(float lat, float lon)
{ {
this.lat = lat; this.lat = lat;
this.lon = lon; this.lon = lon;
this.edges = new(); this.edges = new();
this.previousNode = nullnode;
this.goalDistance = float.MaxValue;
this.pathLength = float.MaxValue;
} }
public static Node nullnode = new(float.NaN, float.NaN);
public Edge? GetEdgeToNode(Node n)
{
foreach (Edge e in this.edges)
if (e.neighbor == n)
return e;
return null;
}
} }
} }

12
Graph/Utils.cs
View File

@ -1,15 +1,15 @@
namespace Graph{ namespace Graph.Utils
{
public struct Utils public struct Utils
{ {
public static float DistanceBetweenNodes(Node n1, Node n2) public static double DistanceBetweenNodes(Node n1, Node n2)
{ {
return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon); return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
} }
public static float DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2) public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
{ {
const int earthRadius = 6371; const int earthRadius = 6371000;
double differenceLat = DegreesToRadians(lat2 - lat1); double differenceLat = DegreesToRadians(lat2 - lat1);
double differenceLon = DegreesToRadians(lon2 - lon1); double differenceLon = DegreesToRadians(lon2 - lon1);
@ -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 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)); double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
return Convert.ToSingle(earthRadius * c); return earthRadius * c;
} }
private static double DegreesToRadians(double deg) private static double DegreesToRadians(double deg)

82
OpenStreetMap Importer/Importer.cs
View File

@ -4,6 +4,7 @@
using Logging; using Logging;
using System.Xml; using System.Xml;
using Graph; using Graph;
using Graph.Utils;
namespace OpenStreetMap_Importer namespace OpenStreetMap_Importer
{ {
@ -16,29 +17,30 @@ namespace OpenStreetMap_Importer
IgnoreComments = true 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" * Count Node occurances when tag is "highway"
*/ */
logger?.Log(LogLevel.INFO, "Counting Node-Occurances..."); logger?.Log(LogLevel.DEBUG, "Opening File...");
Stream mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map); Stream mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map);
logger?.Log(LogLevel.INFO, "Counting Node-Occurances...");
Dictionary<ulong, ushort> occuranceCount = CountNodeOccurances(mapData, logger); Dictionary<ulong, ushort> occuranceCount = CountNodeOccurances(mapData, logger);
mapData.Close();
logger?.Log(LogLevel.DEBUG, "Way Nodes: {0}", occuranceCount.Count); logger?.Log(LogLevel.DEBUG, "Way Nodes: {0}", occuranceCount.Count);
/* /*
* Import Nodes and Edges * Import Nodes and Edges
*/ */
mapData.Position = 0;
logger?.Log(LogLevel.INFO, "Importing Graph..."); logger?.Log(LogLevel.INFO, "Importing Graph...");
mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map); Graph.Graph _graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
Dictionary<ulong, Node> graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger); logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", _graph.GetNodeCount());
mapData.Close(); mapData.Close();
occuranceCount.Clear(); occuranceCount.Clear();
GC.Collect(); GC.Collect();
logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count);
return graph; return _graph;
} }
private static Dictionary<ulong, ushort> CountNodeOccurances(Stream mapData, Logger? logger = null) private static Dictionary<ulong, ushort> CountNodeOccurances(Stream mapData, Logger? logger = null)
@ -57,6 +59,7 @@ namespace OpenStreetMap_Importer
_isHighway = false; _isHighway = false;
_currentIds.Clear(); _currentIds.Clear();
_wayReader = _reader.ReadSubtree(); _wayReader = _reader.ReadSubtree();
logger?.Log(LogLevel.VERBOSE, "WAY: {0}", _reader.GetAttribute("id"));
while (_wayReader.Read()) while (_wayReader.Read())
{ {
if (_reader.Name == "tag" && _reader.GetAttribute("k").Equals("highway")) if (_reader.Name == "tag" && _reader.GetAttribute("k").Equals("highway"))
@ -65,6 +68,7 @@ namespace OpenStreetMap_Importer
{ {
if (!Enum.Parse(typeof(Way.type), _reader.GetAttribute("v"), true).Equals(Way.type.NONE)) if (!Enum.Parse(typeof(Way.type), _reader.GetAttribute("v"), true).Equals(Way.type.NONE))
_isHighway = true; _isHighway = true;
logger?.Log(LogLevel.VERBOSE, "Highway: {0}", _reader.GetAttribute("v"));
} }
catch (ArgumentException) { }; catch (ArgumentException) { };
} }
@ -73,6 +77,7 @@ namespace OpenStreetMap_Importer
try try
{ {
_currentIds.Add(Convert.ToUInt64(_reader.GetAttribute("ref"))); _currentIds.Add(Convert.ToUInt64(_reader.GetAttribute("ref")));
logger?.Log(LogLevel.VERBOSE, "node-ref: {0}", _reader.GetAttribute("ref"));
} }
catch (FormatException) { }; catch (FormatException) { };
} }
@ -93,13 +98,12 @@ namespace OpenStreetMap_Importer
return _occurances; 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> _tempAll = new(); Graph.Graph _graph = new();
Dictionary<ulong, Node> _graph = new();
Way _currentWay; Way _currentWay;
Node _n1, _n2, _currentNode; Node _n1, _n2, _currentNode;
float _weight, _distance = 0; float _time, _distance = 0;
XmlReader _reader = XmlReader.Create(mapData, readerSettings); XmlReader _reader = XmlReader.Create(mapData, readerSettings);
XmlReader _wayReader; XmlReader _wayReader;
@ -114,7 +118,7 @@ namespace OpenStreetMap_Importer
{ {
float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ',')); float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ','));
float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ',')); float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ','));
_tempAll.Add(id, new Node(lat, lon)); _graph.AddNode(id, new Node(lat, lon));
logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]); logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]);
} }
} }
@ -122,6 +126,7 @@ namespace OpenStreetMap_Importer
{ {
_wayReader = _reader.ReadSubtree(); _wayReader = _reader.ReadSubtree();
_currentWay = new(); _currentWay = new();
logger?.Log(LogLevel.VERBOSE, "WAY: {0}", _reader.GetAttribute("id"));
while (_wayReader.Read()) while (_wayReader.Read())
{ {
_wayReader = _reader.ReadSubtree(); _wayReader = _reader.ReadSubtree();
@ -139,7 +144,7 @@ namespace OpenStreetMap_Importer
{ {
ulong _id = Convert.ToUInt64(_reader.GetAttribute("ref")); ulong _id = Convert.ToUInt64(_reader.GetAttribute("ref"));
_currentWay.nodeIds.Add(_id); _currentWay.nodeIds.Add(_id);
logger?.Log(LogLevel.VERBOSE, "nd: {0}", _id); logger?.Log(LogLevel.VERBOSE, "node-ref: {0}", _id);
} }
} }
} }
@ -147,69 +152,62 @@ namespace OpenStreetMap_Importer
if (!_currentWay.GetHighwayType().Equals(Way.type.NONE)) if (!_currentWay.GetHighwayType().Equals(Way.type.NONE))
{ {
logger?.Log(LogLevel.VERBOSE, "WAY Nodes: {0} Type: {1}", _currentWay.nodeIds.Count, _currentWay.GetHighwayType()); logger?.Log(LogLevel.VERBOSE, "WAY Nodes-count: {0} Type: {1}", _currentWay.nodeIds.Count, _currentWay.GetHighwayType());
if (!onlyJunctions) if (!onlyJunctions)
{ {
for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
{ {
_n1 = _tempAll[_currentWay.nodeIds[_nodeIdIndex]]; _n1 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex]);
_n2 = _tempAll[_currentWay.nodeIds[_nodeIdIndex + 1]]; _n2 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex + 1]);
_weight = Utils.DistanceBetweenNodes(_n1, _n2) * 1000 / _currentWay.GetMaxSpeed();
_distance = Convert.ToSingle(Utils.DistanceBetweenNodes(_n1, _n2));
_time = _distance / _currentWay.GetMaxSpeed();
if (!_currentWay.IsOneWay()) if (!_currentWay.IsOneWay())
{ {
_n1.edges.Add(new Edge(_n2, _weight)); _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId()));
_n2.edges.Add(new Edge(_n1, _weight)); _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId()));
} }
else if (_currentWay.IsForward()) else if (_currentWay.IsForward())
{ {
_n1.edges.Add(new Edge(_n2, _weight)); _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId()));
} }
else else
{ {
_n2.edges.Add(new Edge(_n1, _weight)); _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId()));
} }
logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _time);
} }
_graph = _tempAll;
} }
else else
{ {
if (!_tempAll.TryGetValue(_currentWay.nodeIds[0], out _n1)) _n1 = _graph.GetNode(_currentWay.nodeIds[0]);
{
_n1 = _graph[_currentWay.nodeIds[0]];
}
else
{
_graph.TryAdd(_currentWay.nodeIds[0], _n1);
_tempAll.Remove(_currentWay.nodeIds[0]);
}
_currentNode = _n1; _currentNode = _n1;
for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
{ {
if (!_tempAll.TryGetValue(_currentWay.nodeIds[_nodeIdIndex + 1], out _n2)) _n2 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex + 1]);
_n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; _distance += Convert.ToSingle(Utils.DistanceBetweenNodes(_currentNode, _n2));
_distance += Utils.DistanceBetweenNodes(_currentNode, _n2);
if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found
{ {
_weight = _distance * 1000 / _currentWay.GetMaxSpeed(); _time = _distance / _currentWay.GetMaxSpeed();
_distance = 0;
_graph.TryAdd(_currentWay.nodeIds[_nodeIdIndex + 1], _n2);
if (!_currentWay.IsOneWay()) if (!_currentWay.IsOneWay())
{ {
_n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId())); _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId()));
_n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId())); _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId()));
} }
else if (_currentWay.IsForward()) else if (_currentWay.IsForward())
{ {
_n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId())); _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId()));
} }
else else
{ {
_n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId())); _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId()));
} }
_distance = 0;
logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _time);
} }
else else
{ {
_tempAll.Remove(_currentWay.nodeIds[_nodeIdIndex]); _graph.RemoveNode(_currentWay.nodeIds[_nodeIdIndex]); //Not a junction
} }
_currentNode = _n2; _currentNode = _n2;
} }

165
astar/Astar.cs
View File

@ -1,53 +1,39 @@
using Logging; using Logging;
using Graph; using Graph;
using Graph.Utils;
namespace astar namespace astar
{ {
public class Astar public class Astar
{ {
private Dictionary<Node, float> timeRequired = new();
private Dictionary<Node, float> goalDistance = new();
private Dictionary<Node, Node> previousNode = new();
/* public Route FindPath(Graph.Graph graph, Node start, Node goal, Logger? logger)
* Resets the calculated previous nodes and distance to goal
*/
private static void Reset(ref Dictionary<ulong, Node> nodes)
{ {
foreach(Node n in nodes.Values) 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);
{
n.previousNode = Node.nullnode;
n.goalDistance = float.MaxValue;
n.pathLength = float.MaxValue;
}
}
/*
*
*/
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(); List<Node> toVisit = new();
toVisit.Add(start); toVisit.Add(start);
Node currentNode = start; Node currentNode = start;
start.pathLength = 0; SetTimeRequiredToReach(start, 0);
start.goalDistance = Utils.DistanceBetweenNodes(start, goal); SetDistanceToGoal(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)));
while (toVisit.Count > 0 && toVisit[0].pathLength < goal.pathLength) while (toVisit.Count > 0 && GetTimeRequiredToReach(toVisit[0]) < GetTimeRequiredToReach(goal))
{ {
if(currentNode == goal) if(currentNode == goal)
{ {
logger?.Log(LogLevel.INFO, "Way found, checking for shorter option."); logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
} }
currentNode = toVisit.First(); currentNode = toVisit.First();
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.pathLength, 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 //Check all neighbors of current node
foreach (Edge e in currentNode.edges) foreach (Edge e in currentNode.edges)
{ {
if (e.neighbor.pathLength > currentNode.pathLength + e.weight) if (GetTimeRequiredToReach(e.neighbor) > GetTimeRequiredToReach(currentNode) + e.time)
{ {
e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal); SetDistanceToGoal(e.neighbor, Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)));
e.neighbor.pathLength = currentNode.pathLength + e.weight; SetTimeRequiredToReach(e.neighbor, GetTimeRequiredToReach(currentNode) + e.time);
e.neighbor.previousNode = currentNode; SetPreviousNodeOf(e.neighbor, currentNode);
toVisit.Add(e.neighbor); toVisit.Add(e.neighbor);
} }
} }
@ -55,41 +41,62 @@ namespace astar
toVisit.Remove(currentNode); //"Mark" as visited toVisit.Remove(currentNode); //"Mark" as visited
toVisit.Sort(CompareDistance); toVisit.Sort(CompareDistance);
} }
if(goal.previousNode != Node.nullnode)
if(GetPreviousNodeOf(goal) != null)
{ {
logger?.Log(LogLevel.INFO, "Way found, shortest option."); logger?.Log(LogLevel.INFO, "Way found, shortest option.");
currentNode = goal;
} }
else else
{ {
logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon); 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);
return false; return new Route(new List<Step>(), false, float.MaxValue, float.MaxValue);
} }
path.Add(goal); List<Node> tempNodes = new();
tempNodes.Add(goal);
while(currentNode != start) while(currentNode != start)
{ {
path.Add(currentNode.previousNode); #pragma warning disable CS8604, CS8600 // Route was found, so has to have a previous node
currentNode = currentNode.previousNode; tempNodes.Add(GetPreviousNodeOf(currentNode));
currentNode = GetPreviousNodeOf(currentNode);
#pragma warning restore CS8604, CS8600
} }
path.Reverse(); 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]);
steps.Add(new Step(tempNodes[i], e, GetTimeRequiredToReach(tempNodes[i]), GetDistanceToGoal(tempNodes[i])));
#pragma warning restore CS8600, CS8604
totalDistance += e.distance;
}
Route _route = new Route(steps, true, totalDistance, GetTimeRequiredToReach(goal));
logger?.Log(LogLevel.INFO, "Path found"); logger?.Log(LogLevel.INFO, "Path found");
float distance = 0;
Node prev = Node.nullnode; if(logger?.level > LogLevel.INFO)
TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).pathLength);
foreach (Node n in path)
{ {
if(!prev.Equals(Node.nullnode))
float time = 0;
float distance = 0;
logger?.Log(LogLevel.DEBUG, "Route Distance: {0:00000.00km} Time: {1}", _route.distance/1000, TimeSpan.FromSeconds(_route.time));
for(int i = 0; i < _route.steps.Count; i++)
{ {
distance += Utils.DistanceBetweenNodes(prev, n); 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(timeRequired[s.start]), distance/1000);
} }
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 _route;
return true;
} }
/* /*
@ -98,15 +105,15 @@ namespace astar
* 0 => n1 equal n2 * 0 => n1 equal n2
* 1 => n1 larger 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) if (n1 == null || n2 == null)
return 0; return 0;
else else
{ {
if (n1.goalDistance < n2.goalDistance) if (GetDistanceToGoal(n1) < GetDistanceToGoal(n2))
return -1; return -1;
else if (n1.goalDistance > n2.goalDistance) else if (GetDistanceToGoal(n1) > GetDistanceToGoal(n2))
return 1; return 1;
else return 0; else return 0;
} }
@ -118,18 +125,72 @@ namespace astar
* 0 => n1 equal n2 * 0 => n1 equal n2
* 1 => n1 larger 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) if (n1 == null || n2 == null)
return 0; return 0;
else else
{ {
if (n1.pathLength < n2.pathLength) if (GetTimeRequiredToReach(n1) < GetTimeRequiredToReach(n2))
return -1; return -1;
else if (n1.pathLength > n2.pathLength) else if (GetTimeRequiredToReach(n1) > GetTimeRequiredToReach(n2))
return 1; return 1;
else return 0; else return 0;
} }
} }
private float GetTimeRequiredToReach(Node n)
{
if (timeRequired.TryGetValue(n, out float t))
{
return t;
}
else
{
return float.MaxValue;
}
}
private void SetTimeRequiredToReach(Node n, float t)
{
if (!timeRequired.TryAdd(n, t))
timeRequired[n] = t;
}
private float GetDistanceToGoal(Node n)
{
if (goalDistance.TryGetValue(n, out float t))
{
return t;
}
else
{
return float.MaxValue;
}
}
private void SetDistanceToGoal(Node n, float d)
{
if (!goalDistance.TryAdd(n, d))
goalDistance[n] = d;
}
private Node? GetPreviousNodeOf(Node n)
{
if(previousNode.TryGetValue(n, out Node? t))
{
return t;
}else
{
return null;
}
}
private void SetPreviousNodeOf(Node n, Node p)
{
if (!previousNode.TryAdd(n, p))
previousNode[n] = p;
}
} }
} }

36
astar/Route.cs Normal file
View File

@ -0,0 +1,36 @@
using Graph;
namespace astar
{
public class Route
{
public List<Step> steps { get; }
public bool routeFound { get; }
public float distance { get; }
public float time { get; }
public Route(List<Step> steps, bool routeFound, float distance, float timeRequired)
{
this.steps = steps;
this.routeFound = routeFound;
this.distance = distance;
this.time = timeRequired;
}
}
public struct Step
{
public Node start { get; }
public Edge edge { get; }
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;
}
}
}