Updated to new Structure of regions
This commit is contained in:
parent
db6d6ccaef
commit
c16452068c
@ -13,14 +13,18 @@
|
|||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<Reference Include="Geo-Graph">
|
<PackageReference Include="GlaxArguments" Version="1.1.0" />
|
||||||
<HintPath>C:\Users\glax\source\repos\Geo-Graph\Geo-Graph\bin\Debug\net6.0\Geo-Graph.dll</HintPath>
|
</ItemGroup>
|
||||||
|
|
||||||
|
<ItemGroup>
|
||||||
|
<Reference Include="Graph">
|
||||||
|
<HintPath>..\..\OSM_Graph\OSM_Graph\bin\Debug\net8.0\Graph.dll</HintPath>
|
||||||
</Reference>
|
</Reference>
|
||||||
<Reference Include="Logging">
|
<Reference Include="OSM_Graph">
|
||||||
<HintPath>..\..\Logging\Logging\bin\Debug\net6.0\Logging.dll</HintPath>
|
<HintPath>..\..\OSM_Graph\OSM_Graph\bin\Debug\net8.0\OSM_Graph.dll</HintPath>
|
||||||
</Reference>
|
</Reference>
|
||||||
<Reference Include="OSM-XML-Importer">
|
<Reference Include="OSM_Regions">
|
||||||
<HintPath>C:\Users\glax\source\repos\OSM-XML-Importer\OSM-XML-Importer\bin\Debug\net6.0\OSM-XML-Importer.dll</HintPath>
|
<HintPath>..\..\OSM_Regions\OSM_Regions\bin\Debug\net8.0\OSM_Regions.dll</HintPath>
|
||||||
</Reference>
|
</Reference>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
@ -1,60 +1,80 @@
|
|||||||
using GeoGraph;
|
using System.Diagnostics;
|
||||||
using Logging;
|
using System.Globalization;
|
||||||
using astar;
|
using astar;
|
||||||
using OSM_XML_Importer;
|
using GlaxArguments;
|
||||||
|
using GlaxLogger;
|
||||||
|
using Microsoft.Extensions.Logging;
|
||||||
|
using OSM_Regions;
|
||||||
|
|
||||||
string[] confirmation = { "yes", "1", "true" };
|
Logger logger = new(LogLevel.Debug, consoleOut: Console.Out);
|
||||||
|
|
||||||
Logger logger = new(LogType.CONSOLE, LogLevel.DEBUG);
|
Argument pathArg = new (["-p", "--path"], 1, "Path to OSM-XML-File");
|
||||||
string xmlPath;
|
Argument regionArg = new (["-r", "--regionSize"], 1, "Region-Size");
|
||||||
bool onlyJunctions;
|
Argument importPathArg = new (["-i", "--importPath"], 1, "Region-Directory");
|
||||||
switch (args.Length)
|
Argument routeCoordinateArg = new(["-c", "--route", "--coordinates"], 4, "Start and end coordinates");
|
||||||
|
|
||||||
|
ArgumentFetcher af = new ([pathArg, regionArg, importPathArg, routeCoordinateArg]);
|
||||||
|
|
||||||
|
Dictionary<Argument, string[]> arguments = af.Fetch(args);
|
||||||
|
|
||||||
|
if (!arguments.ContainsKey(regionArg))
|
||||||
{
|
{
|
||||||
case 0:
|
PrintUsage();
|
||||||
xmlPath = @"";
|
return;
|
||||||
onlyJunctions = true;
|
}
|
||||||
break;
|
if (!float.TryParse(arguments[regionArg][0], NumberFormatInfo.InvariantInfo, out float regionSize))
|
||||||
case 1:
|
{
|
||||||
xmlPath = args[0];
|
logger.LogError($"Failed to parse region Size from input w{arguments[regionArg][0]}");
|
||||||
onlyJunctions = true;
|
return;
|
||||||
if (!File.Exists(xmlPath))
|
|
||||||
{
|
|
||||||
logger.Log(LogLevel.INFO, "File {0} does not exist.", xmlPath);
|
|
||||||
throw new FileNotFoundException(xmlPath);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 2:
|
|
||||||
xmlPath = args[0];
|
|
||||||
if (!File.Exists(xmlPath))
|
|
||||||
{
|
|
||||||
logger.Log(LogLevel.INFO, "File {0} does not exist.", xmlPath);
|
|
||||||
throw new FileNotFoundException(xmlPath);
|
|
||||||
}
|
|
||||||
if (confirmation.Contains(args[1].ToLower()))
|
|
||||||
onlyJunctions = true;
|
|
||||||
else
|
|
||||||
onlyJunctions = false;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
logger.Log(LogLevel.INFO, "Invalid Arguments.");
|
|
||||||
logger.Log(LogLevel.INFO, "Arguments can be:");
|
|
||||||
logger.Log(LogLevel.INFO, "arg0 Path to file: string");
|
|
||||||
logger.Log(LogLevel.INFO, "arg1 onlyJunctions: 'yes', '1', 'true'");
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Graph graph = Importer.Import(xmlPath, onlyJunctions, logger);
|
if (!arguments.ContainsKey(routeCoordinateArg))
|
||||||
|
|
||||||
Random r = new();
|
|
||||||
Route _route;
|
|
||||||
Node n1, n2;
|
|
||||||
do
|
|
||||||
{
|
{
|
||||||
do
|
PrintUsage();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (!float.TryParse(arguments[routeCoordinateArg][0], NumberFormatInfo.InvariantInfo, out float startLat) ||
|
||||||
|
!float.TryParse(arguments[routeCoordinateArg][1], NumberFormatInfo.InvariantInfo, out float startLon) ||
|
||||||
|
!float.TryParse(arguments[routeCoordinateArg][2], NumberFormatInfo.InvariantInfo, out float endLat) ||
|
||||||
|
!float.TryParse(arguments[routeCoordinateArg][3], NumberFormatInfo.InvariantInfo, out float endLon) )
|
||||||
|
{
|
||||||
|
logger.LogError($"Failed to parse start/end coordinates.");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
string? importPath = null;
|
||||||
|
if (arguments.TryGetValue(importPathArg, out string[]? importPathVal))
|
||||||
|
{
|
||||||
|
importPath = importPathVal[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (arguments.TryGetValue(pathArg, out string[]? pathValue))
|
||||||
|
{
|
||||||
|
if(!File.Exists(pathValue[0]))
|
||||||
{
|
{
|
||||||
n1 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1));
|
logger.LogError($"File doesn't exist {pathValue[0]}");
|
||||||
n2 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1));
|
PrintUsage();
|
||||||
_route = new Astar().FindPath(graph, n1, n2, logger);
|
return;
|
||||||
} while (!_route.routeFound);
|
}
|
||||||
logger.Log(LogLevel.INFO, "Press Enter to find new path.");
|
|
||||||
} while (Console.ReadKey().Key.Equals(ConsoleKey.Enter));
|
Converter converter = new (regionSize, importPath, logger: logger);
|
||||||
|
converter.SplitOsmExportIntoRegionFiles(pathValue[0]);
|
||||||
|
}
|
||||||
|
|
||||||
|
Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, importPath, logger);
|
||||||
|
if(route.RouteFound)
|
||||||
|
Console.WriteLine($"{string.Join("\n", route.Steps)}\n" +
|
||||||
|
$"Distance: {route.Distance:000000.00}m");
|
||||||
|
else
|
||||||
|
Console.WriteLine("No route found.");
|
||||||
|
|
||||||
|
Console.WriteLine($"Visited Nodes: {route.Graph.Nodes.Values.Count(node => node.Previous is not null)}");
|
||||||
|
|
||||||
|
|
||||||
|
void PrintUsage()
|
||||||
|
{
|
||||||
|
Console.WriteLine($"Usage: {Process.GetCurrentProcess().MainModule?.FileName} <-r regionSize> <-c startLat startLon endLat endLon> <options>\n" +
|
||||||
|
$"Options:\n" +
|
||||||
|
$"\t-h onlyHighways\n" +
|
||||||
|
$"\t-p Path to OSM-XML file to split into regions");
|
||||||
|
}
|
375
astar/Astar.cs
375
astar/Astar.cs
@ -1,191 +1,240 @@
|
|||||||
using Logging;
|
using Microsoft.Extensions.Logging;
|
||||||
using GeoGraph;
|
using Graph.Utils;
|
||||||
using GeoGraph.Utils;
|
using OSM_Regions;
|
||||||
|
|
||||||
namespace astar
|
namespace astar
|
||||||
{
|
{
|
||||||
public class Astar
|
public static class Astar
|
||||||
{
|
{
|
||||||
private Dictionary<Node, float> timeRequired = new();
|
public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, string? importFolderPath = null,
|
||||||
private Dictionary<Node, float> goalDistance = new();
|
ILogger? logger = null)
|
||||||
private Dictionary<Node, Node> previousNode = new();
|
|
||||||
|
|
||||||
public Route FindPath(Graph graph, Node start, Node goal, Logger? logger)
|
|
||||||
{
|
{
|
||||||
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);
|
RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
|
||||||
List<Node> toVisit = new();
|
Graph graph = Spiral(rl, startLat, startLon, regionSize);
|
||||||
toVisit.Add(start);
|
KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon);
|
||||||
Node currentNode = start;
|
startNode.Value.PreviousIsFromStart = true;
|
||||||
SetTimeRequiredToReach(start, 0);
|
startNode.Value.Previous = new KeyValuePair<ulong, float>(startNode.Key, 0f);
|
||||||
SetDistanceToGoal(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)));
|
|
||||||
while (toVisit.Count > 0 && GetTimeRequiredToReach(toVisit[0]) < GetTimeRequiredToReach(goal))
|
Graph endRegion = Spiral(rl, endLat, endLon, regionSize);
|
||||||
|
graph.ConcatGraph(endRegion);
|
||||||
|
KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon);
|
||||||
|
endNode.Value.PreviousIsFromStart = false;
|
||||||
|
endNode.Value.Previous = new KeyValuePair<ulong, float>(endNode.Key, 0f);
|
||||||
|
|
||||||
|
logger?.Log(LogLevel.Information,
|
||||||
|
"From {0:00.00000}#{1:000.00000} to {2:00.00000}#{3:000.00000} Great-Circle {4:00000.00}km",
|
||||||
|
startNode.Value.Lat, startNode.Value.Lon, endNode.Value.Lat, endNode.Value.Lon,
|
||||||
|
NodeUtils.DistanceBetween(startNode.Value, endNode.Value) / 1000);
|
||||||
|
|
||||||
|
PriorityQueue<ulong, double> toVisitStart = new();
|
||||||
|
toVisitStart.Enqueue(startNode.Key, NodeUtils.DistanceBetween(startNode.Value, endNode.Value));
|
||||||
|
PriorityQueue<ulong, double> toVisitEnd = new();
|
||||||
|
toVisitEnd.Enqueue(endNode.Key, NodeUtils.DistanceBetween(endNode.Value, startNode.Value));
|
||||||
|
|
||||||
|
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
|
||||||
{
|
{
|
||||||
currentNode = toVisit.First();
|
logger?.LogDebug($"Length toVisit-Start: {toVisitStart.Count} -End: {toVisitEnd.Count}");
|
||||||
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
|
* FROM START
|
||||||
foreach (Edge e in currentNode.edges)
|
*/
|
||||||
|
ulong currentNodeStartId = toVisitStart.Dequeue();
|
||||||
|
Node? currentNodeStart;
|
||||||
|
if (!graph.ContainsNode(currentNodeStartId))
|
||||||
{
|
{
|
||||||
if (GetTimeRequiredToReach(e.neighbor) > GetTimeRequiredToReach(currentNode) + e.time)
|
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(currentNodeStartId));
|
||||||
|
if (newRegion is null)
|
||||||
{
|
{
|
||||||
SetDistanceToGoal(e.neighbor, Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)));
|
logger?.LogError($"Could not load Region for Node {currentNodeStartId}");
|
||||||
SetTimeRequiredToReach(e.neighbor, GetTimeRequiredToReach(currentNode) + e.time);
|
currentNodeStart = null;
|
||||||
SetPreviousNodeOf(e.neighbor, currentNode);
|
}
|
||||||
if (!toVisit.Contains(e.neighbor))
|
else
|
||||||
toVisit.Add(e.neighbor);
|
{
|
||||||
|
graph.ConcatGraph(newRegion);
|
||||||
|
currentNodeStart = graph.Nodes[currentNodeStartId];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
else
|
||||||
toVisit.Remove(currentNode); //"Mark" as visited
|
currentNodeStart = graph.Nodes[currentNodeStartId];
|
||||||
toVisit.Sort(CompareDistance);
|
logger?.LogTrace($"Current Node Start: {currentNodeStartId} {currentNodeStart}");
|
||||||
}
|
if (currentNodeStart is not null)
|
||||||
|
|
||||||
if(GetPreviousNodeOf(goal) != null)
|
|
||||||
{
|
|
||||||
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
|
|
||||||
currentNode = goal;
|
|
||||||
}
|
|
||||||
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);
|
|
||||||
return new Route(new List<Step>(), false, float.MaxValue, float.MaxValue);
|
|
||||||
}
|
|
||||||
|
|
||||||
#pragma warning disable CS8604, CS8600 // Route was found, so has to have a previous node and edges
|
|
||||||
List<Node> tempNodes = new();
|
|
||||||
tempNodes.Add(goal);
|
|
||||||
while(currentNode != start)
|
|
||||||
{
|
|
||||||
tempNodes.Add(GetPreviousNodeOf(currentNode));
|
|
||||||
currentNode = GetPreviousNodeOf(currentNode);
|
|
||||||
}
|
|
||||||
tempNodes.Reverse();
|
|
||||||
|
|
||||||
List<Step> steps = new();
|
|
||||||
float totalDistance = 0;
|
|
||||||
|
|
||||||
for(int i = 0; i < tempNodes.Count - 1; i++)
|
|
||||||
{
|
|
||||||
Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]);
|
|
||||||
steps.Add(new Step(tempNodes[i], e, GetTimeRequiredToReach(tempNodes[i]), GetDistanceToGoal(tempNodes[i])));
|
|
||||||
totalDistance += e.distance;
|
|
||||||
}
|
|
||||||
|
|
||||||
Route _route = new Route(steps, true, totalDistance, GetTimeRequiredToReach(goal));
|
|
||||||
|
|
||||||
logger?.Log(LogLevel.INFO, "Path found");
|
|
||||||
|
|
||||||
if(logger?.level > LogLevel.INFO)
|
|
||||||
{
|
|
||||||
|
|
||||||
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++)
|
|
||||||
{
|
{
|
||||||
Step s = _route.steps[i];
|
foreach ((ulong nodeId, ulong wayId) in currentNodeStart.Neighbors)
|
||||||
time += s.edge.time;
|
{
|
||||||
distance += s.edge.distance;
|
//TODO checks for way-stuff
|
||||||
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);
|
Node? neighbor;
|
||||||
|
if (!graph.ContainsNode(nodeId))
|
||||||
|
{
|
||||||
|
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(nodeId));
|
||||||
|
if (newRegion is null)
|
||||||
|
{
|
||||||
|
logger?.LogError($"Could not load Region for Node {nodeId}");
|
||||||
|
neighbor = null;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
graph.ConcatGraph(newRegion);
|
||||||
|
neighbor = Node.FromGraphNode(graph.Nodes[nodeId]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
neighbor = graph.Nodes[nodeId];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (neighbor is not null)
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
* IMPORTANT SHIT BELOW
|
||||||
|
*/
|
||||||
|
if (neighbor.PreviousIsFromStart is false)//Check if we found the opposite End
|
||||||
|
return PathFound(graph, currentNodeStart, neighbor);
|
||||||
|
float distance = currentNodeStart.Previous!.Value.Value + (float)neighbor.DistanceTo(currentNodeStart);
|
||||||
|
if (neighbor.Previous is null || neighbor.Previous.Value.Value > distance)
|
||||||
|
{
|
||||||
|
neighbor.Previous = new KeyValuePair<ulong, float>(currentNodeStartId, distance);
|
||||||
|
neighbor.PreviousIsFromStart = true;
|
||||||
|
toVisitStart.Enqueue(nodeId, NodeUtils.DistanceBetween(neighbor, endNode.Value));
|
||||||
|
}
|
||||||
|
logger?.LogTrace($"Neighbor {nodeId} {neighbor}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* FROM END
|
||||||
|
*/
|
||||||
|
ulong currentNodeEndId = toVisitEnd.Dequeue();
|
||||||
|
Node? currentNodeEnd;
|
||||||
|
if (!graph.ContainsNode(currentNodeEndId))
|
||||||
|
{
|
||||||
|
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(currentNodeEndId));
|
||||||
|
if (newRegion is null)
|
||||||
|
{
|
||||||
|
logger?.LogError($"Could not load Region for Node {currentNodeEndId}");
|
||||||
|
currentNodeEnd = null;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
graph.ConcatGraph(newRegion);
|
||||||
|
currentNodeEnd = graph.Nodes[currentNodeEndId];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
currentNodeEnd = graph.Nodes[currentNodeEndId];
|
||||||
|
logger?.LogTrace($"Current Node End: {currentNodeEndId} {currentNodeEnd}");
|
||||||
|
|
||||||
|
if (currentNodeEnd is not null)
|
||||||
|
{
|
||||||
|
foreach ((ulong nodeId, ulong wayId) in currentNodeEnd.Neighbors)
|
||||||
|
{
|
||||||
|
//TODO checks for way-stuff
|
||||||
|
Node? neighbor;
|
||||||
|
if (!graph.ContainsNode(nodeId))
|
||||||
|
{
|
||||||
|
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(nodeId));
|
||||||
|
if (newRegion is null)
|
||||||
|
{
|
||||||
|
logger?.LogError($"Could not load Region for Node {nodeId}");
|
||||||
|
neighbor = null;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
graph.ConcatGraph(newRegion);
|
||||||
|
neighbor = Node.FromGraphNode(graph.Nodes[nodeId]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
neighbor = graph.Nodes[nodeId];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (neighbor is not null)
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
* IMPORTANT SHIT BELOW
|
||||||
|
*/
|
||||||
|
if (neighbor.PreviousIsFromStart is true)//Check if we found the opposite End
|
||||||
|
return PathFound(graph, neighbor, currentNodeEnd);
|
||||||
|
|
||||||
|
float distance = currentNodeEnd.Previous!.Value.Value + (float)neighbor.DistanceTo(currentNodeEnd);
|
||||||
|
if (neighbor.Previous is null || neighbor.Previous.Value.Value > distance)
|
||||||
|
{
|
||||||
|
neighbor.Previous = new KeyValuePair<ulong, float>(currentNodeEndId, distance);
|
||||||
|
neighbor.PreviousIsFromStart = false;
|
||||||
|
toVisitEnd.Enqueue(nodeId, NodeUtils.DistanceBetween(neighbor, startNode.Value));
|
||||||
|
}
|
||||||
|
logger?.LogTrace($"Neighbor {nodeId} {neighbor}");
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
return new Route(graph, Array.Empty<Step>().ToList(), false);
|
||||||
return _route;
|
|
||||||
#pragma warning restore CS8604, CS8600
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
private static Route PathFound(Graph graph, Node fromStart, Node fromEnd)
|
||||||
* Compares two nodes and returns the node closer to the goal
|
|
||||||
* -1 => n1 smaller n2
|
|
||||||
* 0 => n1 equal n2
|
|
||||||
* 1 => n1 larger n2
|
|
||||||
*/
|
|
||||||
private int CompareDistance(Node n1, Node n2)
|
|
||||||
{
|
{
|
||||||
if (n1 == null || n2 == null)
|
List<Step> path = new();
|
||||||
return 0;
|
Node current = fromStart;
|
||||||
else
|
while (current.Previous is not null && current.Previous.Value.Value == 0f)
|
||||||
{
|
{
|
||||||
if (GetDistanceToGoal(n1) < GetDistanceToGoal(n2))
|
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[current.Previous.Value.Key], current), graph.Nodes[current.Previous.Value.Key], current);
|
||||||
return -1;
|
path.Add(step);
|
||||||
else if (GetDistanceToGoal(n1) > GetDistanceToGoal(n2))
|
current = graph.Nodes[current.Previous.Value.Key];
|
||||||
return 1;
|
|
||||||
else return 0;
|
|
||||||
}
|
}
|
||||||
}
|
path.Reverse();//Since we go from the middle backwards until here
|
||||||
|
path.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, fromEnd));
|
||||||
/*
|
current = fromEnd;
|
||||||
* Compares two nodes and returns the node with the shorter path
|
while (current.Previous is not null && current.Previous.Value.Value == 0f)
|
||||||
* -1 => n1 smaller n2
|
|
||||||
* 0 => n1 equal n2
|
|
||||||
* 1 => n1 larger n2
|
|
||||||
*/
|
|
||||||
private int ComparePathLength(Node n1, Node n2)
|
|
||||||
{
|
|
||||||
if (n1 == null || n2 == null)
|
|
||||||
return 0;
|
|
||||||
else
|
|
||||||
{
|
{
|
||||||
if (GetTimeRequiredToReach(n1) < GetTimeRequiredToReach(n2))
|
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[current.Previous.Value.Key], current), current, graph.Nodes[current.Previous.Value.Key]);
|
||||||
return -1;
|
path.Add(step);
|
||||||
else if (GetTimeRequiredToReach(n1) > GetTimeRequiredToReach(n2))
|
current = graph.Nodes[current.Previous.Value.Key];
|
||||||
return 1;
|
|
||||||
else return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return new Route(graph, path, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private static Graph Spiral(RegionLoader loader, float lat, float lon, float regionSize)
|
||||||
private float GetTimeRequiredToReach(Node n)
|
|
||||||
{
|
{
|
||||||
if (timeRequired.TryGetValue(n, out float t))
|
Graph? ret = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat, lon));
|
||||||
|
int iteration = 1;
|
||||||
|
while (ret is null)
|
||||||
{
|
{
|
||||||
return t;
|
for (int x = -iteration; x <= iteration; x++)
|
||||||
|
{
|
||||||
|
Graph? g1 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat + x * regionSize, lon - iteration * regionSize));
|
||||||
|
Graph? g2 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat + x * regionSize, lon + iteration * regionSize));
|
||||||
|
if (ret is not null)
|
||||||
|
{
|
||||||
|
ret.ConcatGraph(g1);
|
||||||
|
ret.ConcatGraph(g2);
|
||||||
|
}
|
||||||
|
else if (ret is null && g1 is not null)
|
||||||
|
{
|
||||||
|
ret = g1;
|
||||||
|
ret.ConcatGraph(g2);
|
||||||
|
}else if (ret is null && g2 is not null)
|
||||||
|
ret = g2;
|
||||||
|
}
|
||||||
|
for (int y = -iteration + 1; y < iteration; y++)
|
||||||
|
{
|
||||||
|
Graph? g1 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat - iteration * regionSize, lon + y * regionSize));
|
||||||
|
Graph? g2 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat + iteration * regionSize, lon + y * regionSize));
|
||||||
|
if (ret is not null)
|
||||||
|
{
|
||||||
|
ret.ConcatGraph(g1);
|
||||||
|
ret.ConcatGraph(g2);
|
||||||
|
}
|
||||||
|
else if (ret is null && g1 is not null)
|
||||||
|
{
|
||||||
|
ret = g1;
|
||||||
|
ret.ConcatGraph(g2);
|
||||||
|
}else if (ret is null && g2 is not null)
|
||||||
|
ret = g2;
|
||||||
|
}
|
||||||
|
iteration++;
|
||||||
}
|
}
|
||||||
else
|
return ret;
|
||||||
{
|
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
61
astar/Graph.cs
Normal file
61
astar/Graph.cs
Normal file
@ -0,0 +1,61 @@
|
|||||||
|
using Graph;
|
||||||
|
|
||||||
|
namespace astar;
|
||||||
|
|
||||||
|
public class Graph
|
||||||
|
{
|
||||||
|
public readonly Dictionary<ulong, Node> Nodes = new();
|
||||||
|
public readonly Dictionary<ulong, Way> Ways = new ();
|
||||||
|
|
||||||
|
public static Graph? FromGraph(global::Graph.Graph? graph)
|
||||||
|
{
|
||||||
|
if (graph is null)
|
||||||
|
return null;
|
||||||
|
Graph ret = new();
|
||||||
|
foreach ((ulong id, global::Graph.Node? node) in graph.Nodes)
|
||||||
|
ret.Nodes.Add(id, Node.FromGraphNode(node));
|
||||||
|
foreach ((ulong id, Way? way) in graph.Ways)
|
||||||
|
ret.Ways.Add(id, way);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ConcatGraph(Graph? graph)
|
||||||
|
{
|
||||||
|
if (graph is null)
|
||||||
|
return;
|
||||||
|
foreach ((ulong id, Node n) in graph.Nodes)
|
||||||
|
this.Nodes.TryAdd(id, n);
|
||||||
|
foreach ((ulong id, Way w) in graph.Ways)
|
||||||
|
this.Ways.TryAdd(id, w);
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool ContainsNode(Node node)
|
||||||
|
{
|
||||||
|
return Nodes.ContainsValue(node);
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool ContainsNode(ulong nodeId)
|
||||||
|
{
|
||||||
|
return Nodes.ContainsKey(nodeId);
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool ContainsWay(Way way)
|
||||||
|
{
|
||||||
|
return Ways.ContainsValue(way);
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool ContainsWay(ulong wayId)
|
||||||
|
{
|
||||||
|
return Ways.ContainsKey(wayId);
|
||||||
|
}
|
||||||
|
|
||||||
|
public KeyValuePair<ulong, Node> ClosestNodeToCoordinates(float lat, float lon)
|
||||||
|
{
|
||||||
|
return Nodes.MinBy(n => n.Value.DistanceTo(lat, lon));
|
||||||
|
}
|
||||||
|
|
||||||
|
public override string ToString()
|
||||||
|
{
|
||||||
|
return $"Graph {Nodes.Count} Nodes {Ways.Count} Ways.";
|
||||||
|
}
|
||||||
|
}
|
14
astar/Node.cs
Normal file
14
astar/Node.cs
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
namespace astar;
|
||||||
|
|
||||||
|
public class Node(float lat, float lon, Dictionary<ulong, ulong>? neighbors = null) : global::Graph.Node(lat, lon, neighbors)
|
||||||
|
{
|
||||||
|
public KeyValuePair<ulong, float>? Previous = null;
|
||||||
|
public bool? PreviousIsFromStart = null;
|
||||||
|
|
||||||
|
public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors);
|
||||||
|
|
||||||
|
public override string ToString()
|
||||||
|
{
|
||||||
|
return $"{Lat:00.000000} {Lon:000.000000} Previous {Previous?.Key} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
|
||||||
|
}
|
||||||
|
}
|
@ -1,36 +1,57 @@
|
|||||||
using GeoGraph;
|
namespace astar
|
||||||
namespace astar
|
|
||||||
{
|
{
|
||||||
public class Route
|
public class Route(Graph graph, List<Step> steps, bool routeFound)
|
||||||
{
|
{
|
||||||
public List<Step> steps { get; }
|
public Graph Graph { get; } = graph;
|
||||||
public bool routeFound { get; }
|
public List<Step> Steps { get; } = steps;
|
||||||
public float distance { get; }
|
public bool RouteFound { get; } = routeFound;
|
||||||
public float time { get; }
|
public float Distance => Steps.Sum(step => step.Distance);
|
||||||
|
|
||||||
|
public KeyValuePair<float, float> MinCoordinates()
|
||||||
public Route(List<Step> steps, bool routeFound, float distance, float timeRequired)
|
|
||||||
{
|
{
|
||||||
this.steps = steps;
|
float minLat, minLon;
|
||||||
this.routeFound = routeFound;
|
if (RouteFound)
|
||||||
this.distance = distance;
|
{
|
||||||
this.time = timeRequired;
|
Step minLatStep = Steps.MinBy(step => step.Node1.Lat < step.Node2.Lat ? step.Node1.Lat : step.Node2.Lat);
|
||||||
|
Step minLonStep = Steps.MinBy(step => step.Node1.Lon < step.Node2.Lon ? step.Node1.Lon : step.Node2.Lon);
|
||||||
|
minLat = minLatStep.Node1.Lat < minLatStep.Node2.Lat ? minLatStep.Node1.Lat : minLatStep.Node2.Lat;
|
||||||
|
minLon = minLonStep.Node1.Lon < minLonStep.Node2.Lon ? minLonStep.Node1.Lon : minLonStep.Node2.Lon;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
minLat = Graph.Nodes.MinBy(node => node.Value.Lat).Value.Lat;
|
||||||
|
minLon = Graph.Nodes.MinBy(node => node.Value.Lon).Value.Lon;
|
||||||
|
}
|
||||||
|
return new KeyValuePair<float, float>(minLat, minLon);
|
||||||
|
}
|
||||||
|
|
||||||
|
public KeyValuePair<float, float> MaxCoordinates()
|
||||||
|
{
|
||||||
|
float maxLat, maxLon;
|
||||||
|
if (RouteFound)
|
||||||
|
{
|
||||||
|
Step maxLatStep = Steps.MaxBy(step => step.Node1.Lat > step.Node2.Lat ? step.Node1.Lat : step.Node2.Lat);
|
||||||
|
Step maxLonStep = Steps.MaxBy(step => step.Node1.Lon > step.Node2.Lon ? step.Node1.Lon : step.Node2.Lon);
|
||||||
|
maxLat = maxLatStep.Node1.Lat > maxLatStep.Node2.Lat ? maxLatStep.Node1.Lat : maxLatStep.Node2.Lat;
|
||||||
|
maxLon = maxLonStep.Node1.Lon > maxLonStep.Node2.Lon ? maxLonStep.Node1.Lon : maxLonStep.Node2.Lon;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
maxLat = Graph.Nodes.MaxBy(node => node.Value.Lat).Value.Lat;
|
||||||
|
maxLon = Graph.Nodes.MaxBy(node => node.Value.Lon).Value.Lon;
|
||||||
|
}
|
||||||
|
return new KeyValuePair<float, float>(maxLat, maxLon);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public struct Step
|
public struct Step(float distance, Node node1, Node node2)
|
||||||
{
|
{
|
||||||
public Node start { get; }
|
public readonly Node Node1 = node1, Node2 = node2;
|
||||||
public Edge edge { get; }
|
public readonly float Distance = distance;
|
||||||
|
|
||||||
public float timeRequired { get; }
|
public override string ToString()
|
||||||
public float goalDistance { get; }
|
|
||||||
public Step(Node start, Edge route, float timeRequired, float goalDistance)
|
|
||||||
{
|
{
|
||||||
this.start = start;
|
return $"{Node1.Lat:00.000000} {Node1.Lon:000.000000} --- {Distance:0000.00}m ---> {Node2.Lat:00.000000} {Node2.Lon:000.000000}";
|
||||||
this.edge = route;
|
|
||||||
this.timeRequired = timeRequired;
|
|
||||||
this.goalDistance = goalDistance;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -8,11 +8,19 @@
|
|||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<Reference Include="Geo-Graph">
|
<PackageReference Include="GlaxLogger" Version="1.0.7.2" />
|
||||||
<HintPath>C:\Users\glax\source\repos\Geo-Graph\Geo-Graph\bin\Debug\net6.0\Geo-Graph.dll</HintPath>
|
<PackageReference Include="Microsoft.Extensions.Logging" Version="8.0.0" />
|
||||||
|
</ItemGroup>
|
||||||
|
|
||||||
|
<ItemGroup>
|
||||||
|
<Reference Include="Graph">
|
||||||
|
<HintPath>..\..\OSM_Graph\OSM_Graph\bin\Debug\net8.0\Graph.dll</HintPath>
|
||||||
</Reference>
|
</Reference>
|
||||||
<Reference Include="Logging">
|
<Reference Include="OSM_Graph">
|
||||||
<HintPath>..\..\Logging\Logging\bin\Debug\net6.0\Logging.dll</HintPath>
|
<HintPath>..\..\OSM_Graph\OSM_Graph\bin\Debug\net8.0\OSM_Graph.dll</HintPath>
|
||||||
|
</Reference>
|
||||||
|
<Reference Include="OSM_Regions">
|
||||||
|
<HintPath>..\..\OSM_Regions\OSM_Regions\bin\Debug\net8.0\OSM_Regions.dll</HintPath>
|
||||||
</Reference>
|
</Reference>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
@ -1,2 +1,8 @@
|
|||||||
<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
|
<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
|
||||||
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005CGeo_002DGraph_005CGeo_002DGraph_005Cbin_005CDebug_005Cnet6_002E0_005CGeo_002DGraph_002Edll/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005CGeo_002DGraph_005CGeo_002DGraph_005Cbin_005CDebug_005Cnet6_002E0_005CGeo_002DGraph_002Edll/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005COSM_002DXML_002DImporter_005COSM_002DXML_002DImporter_005Cbin_005CDebug_005Cnet8_002E0_005COSM_002DXML_002DImporter_002Edll/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005COSM_005FGraph_005COSM_005FGraph_005Cbin_005CDebug_005Cnet8_002E0_005CGraph_002Edll/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005COSM_005FGraph_005COSM_005FGraph_005Cbin_005CDebug_005Cnet8_002E0_005COSM_005FGraph_002Edll/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005COSM_005FRegions_005COSM_005FRegions_005Cbin_005CDebug_005Cnet8_002E0_005CGraph_002Edll/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005COSM_005FRegions_005COSM_005FRegions_005Cbin_005CDebug_005Cnet8_002E0_005COSM_005FGraph_002Edll/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005COSM_005FRegions_005COSM_005FRegions_005Cbin_005CDebug_005Cnet8_002E0_005COSM_005FRegions_002Edll/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>
|
Loading…
Reference in New Issue
Block a user