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

Switchable between distance and speed metric

Browse Source 
Fix Priority Angle rating
This commit is contained in:
glax 2024-07-23 19:10:29 +02:00
parent 692fb93a94
commit 57a6838f86
6 changed files with 86 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Executable/Program.cs
View File

@ -61,7 +61,7 @@ if (arguments.TryGetValue(pathArg, out string[]? pathValue))
converter.SplitOsmExportIntoRegionFiles(pathValue[0]); converter.SplitOsmExportIntoRegionFiles(pathValue[0]);
} }
Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, true, importPath, logger); Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, true, importFolderPath: importPath, logger: logger);
if(route.RouteFound) if(route.RouteFound)
Console.WriteLine(route); Console.WriteLine(route);
else else

145
astar/Astar.cs
View File

@ -1,5 +1,4 @@
using astar.PathingHelper; using astar.PathingHelper;
using Graph;
using Microsoft.Extensions.Logging; using Microsoft.Extensions.Logging;
using Graph.Utils; using Graph.Utils;
using OSM_Graph.Enums; using OSM_Graph.Enums;
@ -9,7 +8,7 @@ namespace astar
{ {
public static class Astar public static class Astar
{ {
public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, string? importFolderPath = null, public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, PathMeasure pathing = PathMeasure.Distance, string? importFolderPath = null,
ILogger? logger = null) ILogger? logger = null)
{ {
RegionLoader rl = new(regionSize, importFolderPath, logger: logger); RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
@ -19,12 +18,12 @@ namespace astar
KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon, car); KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon, car);
startNode.Value.PreviousIsFromStart = true; startNode.Value.PreviousIsFromStart = true;
startNode.Value.PreviousNodeId = startNode.Key; startNode.Value.PreviousNodeId = startNode.Key;
startNode.Value.Distance = 0f; startNode.Value.Metric = 0f;
KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon, car); KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon, car);
endNode.Value.PreviousIsFromStart = false; endNode.Value.PreviousIsFromStart = false;
endNode.Value.PreviousNodeId = endNode.Key; endNode.Value.PreviousNodeId = endNode.Key;
endNode.Value.Distance = 0f; endNode.Value.Metric = 0f;
double totalDistance = NodeUtils.DistanceBetween(startNode.Value, endNode.Value); double totalDistance = NodeUtils.DistanceBetween(startNode.Value, endNode.Value);
PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetMaxSpeed(car)); PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetMaxSpeed(car));
@ -40,86 +39,76 @@ namespace astar
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0) while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
{ {
ulong currentNodeStartId = toVisitStart.Dequeue(); Route? route = null;
Node currentNodeStart = graph.Nodes[currentNodeStartId]; if (toVisitStart.Count >= toVisitEnd.Count && route is null)
foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNodeStart.Neighbors)
{ {
if (!graph.ContainsNode(neighborId)) for(int i = 0; i < toVisitStart.Count / 10 && route is null; i++)
graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId))); route = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, endNode.Value, car, pathing, logger);
if (!graph.ContainsWay(wayId.Key)) }
if(route is null)
route = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, endNode.Value, car, pathing, logger);
if (toVisitEnd.Count >= toVisitStart.Count && route is null)
{ {
foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key)) for(int i = 0; i < toVisitEnd.Count / 10 && route is null; i++)
graph.ConcatGraph(Graph.FromGraph(g)); route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger);
} }
if(route is null)
route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger);
OSM_Graph.Way way = graph.Ways[wayId.Key]; if (route is not null)
byte speed = SpeedHelper.GetSpeed(way, car); return route;
if(speed < 1) logger?.LogDebug($"toVisit-Queues: {toVisitStart.Count} {toVisitStart.UnorderedItems.MinBy(i => i.Priority).Priority} {toVisitEnd.Count} {toVisitEnd.UnorderedItems.MinBy(i => i.Priority).Priority}");
continue;
if(wayId.Value && way.GetDirection() == WayDirection.Forwards && car)
continue;
if(!wayId.Value && way.GetDirection() == WayDirection.Backwards && car)
continue;
Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End
return PathFound(graph, currentNodeStart, neighborNode, car, logger);
float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
{
neighborNode.PreviousNodeId = currentNodeStartId;
neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = true;
toVisitStart.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeStart, neighborNode, endNode.Value, speed));
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
}
ulong currentNodeEndId = toVisitEnd.Dequeue();
Node currentNodeEnd = graph.Nodes[currentNodeEndId];
foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNodeEnd.Neighbors)
{
if (!graph.ContainsNode(neighborId))
graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
if (!graph.ContainsWay(wayId.Key))
{
foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key))
graph.ConcatGraph(Graph.FromGraph(g));
}
OSM_Graph.Way way = graph.Ways[wayId.Key];
byte speed = SpeedHelper.GetSpeed(way, car);
if(speed < 1)
continue;
if(wayId.Value && way.GetDirection() == WayDirection.Backwards && car)
continue;
if(!wayId.Value && way.GetDirection() == WayDirection.Forwards && car)
continue;
Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End
return PathFound(graph, neighborNode, currentNodeEnd, car, logger);
float distance = (currentNodeEnd.Distance??float.MaxValue) + (float)currentNodeEnd.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
{
neighborNode.PreviousNodeId = currentNodeEndId;
neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = false;
toVisitEnd.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeEnd, neighborNode, startNode.Value, speed));
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
}
logger?.LogDebug($"Distance {currentNodeStart.DistanceTo(currentNodeEnd):000000.00}m toVisit-Queues: {toVisitStart.Count} {toVisitStart.UnorderedItems.MinBy(i => i.Priority).Priority} {toVisitEnd.Count} {toVisitEnd.UnorderedItems.MinBy(i => i.Priority).Priority}");
} }
return new Route(graph, Array.Empty<Step>().ToList(), false); return new Route(graph, Array.Empty<Step>().ToList(), false);
} }
private static Route? ExploreSide(bool fromStart, Graph graph, PriorityQueue<ulong, int> toVisit, RegionLoader rl, PriorityHelper priorityHelper, Node goalNode, bool car, PathMeasure pathing = PathMeasure.Distance, ILogger? logger = null)
{
ulong currentNodeId = toVisit.Dequeue();
Node currentNode = graph.Nodes[currentNodeId];
logger?.LogDebug($"Distance to goal {currentNode.DistanceTo(goalNode):00000.00}m");
foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNode.Neighbors)
{
if (!graph.ContainsNode(neighborId))
graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
if (!graph.ContainsWay(wayId.Key))
{
foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key))
graph.ConcatGraph(Graph.FromGraph(g));
}
OSM_Graph.Way way = graph.Ways[wayId.Key];
byte speed = SpeedHelper.GetSpeed(way, car);
if(speed < 1)
continue;
if(wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Forwards : WayDirection.Backwards) && car)
continue;
if(!wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Backwards : WayDirection.Forwards) && car)
continue;
Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart is not null && neighborNode.PreviousIsFromStart != fromStart)//Check if we found the opposite End
return fromStart ? PathFound(graph, currentNode, neighborNode, car, logger) : PathFound(graph, neighborNode, currentNode, car, logger);
float metric = (currentNode.Metric ?? float.MaxValue) + (pathing is PathMeasure.Distance
? (float)currentNode.DistanceTo(neighborNode)
: (float)currentNode.DistanceTo(neighborNode) / speed);
if (neighborNode.PreviousNodeId is null || neighborNode.Metric > metric)
{
neighborNode.PreviousNodeId = currentNodeId;
neighborNode.Metric = metric;
neighborNode.PreviousIsFromStart = fromStart;
toVisit.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNode, neighborNode, goalNode, speed));
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
}
return null;
}
private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, bool car = true, ILogger? logger = null) private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, bool car = true, ILogger? logger = null)
{ {
logger?.LogInformation("Path found!"); logger?.LogInformation("Path found!");
@ -127,7 +116,7 @@ namespace astar
OSM_Graph.Way toNeighbor = graph.Ways[fromStart.Neighbors.First(n => graph.Nodes[n.Key] == fromEnd).Value.Key]; OSM_Graph.Way toNeighbor = graph.Ways[fromStart.Neighbors.First(n => graph.Nodes[n.Key] == fromEnd).Value.Key];
path.Add(new Step(fromStart, fromEnd, (float)fromStart.DistanceTo(fromEnd), SpeedHelper.GetSpeed(toNeighbor, car))); path.Add(new Step(fromStart, fromEnd, (float)fromStart.DistanceTo(fromEnd), SpeedHelper.GetSpeed(toNeighbor, car)));
Node current = fromStart; Node current = fromStart;
while (current.Distance != 0f) while (current.Metric != 0f)
{ {
Node previous = graph.Nodes[(ulong)current.PreviousNodeId!]; Node previous = graph.Nodes[(ulong)current.PreviousNodeId!];
OSM_Graph.Way previousToCurrent = graph.Ways[previous.Neighbors.First(n => graph.Nodes[n.Key] == current).Value.Key]; OSM_Graph.Way previousToCurrent = graph.Ways[previous.Neighbors.First(n => graph.Nodes[n.Key] == current).Value.Key];
@ -137,7 +126,7 @@ namespace astar
} }
path.Reverse();//Since we go from the middle backwards until here path.Reverse();//Since we go from the middle backwards until here
current = fromEnd; current = fromEnd;
while (current.Distance != 0f) while (current.Metric != 0f)
{ {
Node next = graph.Nodes[(ulong)current.PreviousNodeId!]; Node next = graph.Nodes[(ulong)current.PreviousNodeId!];
OSM_Graph.Way currentToNext = graph.Ways[current.Neighbors.First(n => graph.Nodes[n.Key] == next).Value.Key]; OSM_Graph.Way currentToNext = graph.Ways[current.Neighbors.First(n => graph.Nodes[n.Key] == next).Value.Key];

4
astar/Node.cs
View File

@ -3,13 +3,13 @@
public class Node(float lat, float lon, Dictionary<ulong, KeyValuePair<ulong, bool>>? neighbors = null) : global::Graph.Node(lat, lon, neighbors) public class Node(float lat, float lon, Dictionary<ulong, KeyValuePair<ulong, bool>>? neighbors = null) : global::Graph.Node(lat, lon, neighbors)
{ {
public ulong? PreviousNodeId = null; public ulong? PreviousNodeId = null;
public float? Distance = null; public float? Metric = null;
public bool? PreviousIsFromStart = null; public bool? PreviousIsFromStart = null;
public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors); public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors);
public override string ToString() public override string ToString()
{ {
return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {Distance} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}"; return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {Metric} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
} }
} }

6
astar/PathingHelper/PathMeasure.cs Normal file
View File

@ -0,0 +1,6 @@
namespace astar.PathingHelper;
public enum PathMeasure
{
Distance, Time
}

21
astar/PathingHelper/PriorityHelper.cs
View File

@ -1,6 +1,4 @@
using Graph; namespace astar.PathingHelper;
namespace astar.PathingHelper;
public class PriorityHelper(double totalDistance, byte maxSpeed) public class PriorityHelper(double totalDistance, byte maxSpeed)
{ {
@ -9,17 +7,20 @@ public class PriorityHelper(double totalDistance, byte maxSpeed)
public int CalculatePriority(Node current, Node neighbor, Node goal, byte speed) public int CalculatePriority(Node current, Node neighbor, Node goal, byte speed)
{ {
double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small
/*double currentDistanceToGoal = current.DistanceTo(goal); double currentDistanceToGoal = current.DistanceTo(goal);
double currentDistanceToNeighbor = current.DistanceTo(neighbor); double currentDistanceToNeighbor = current.DistanceTo(neighbor);
double angle = //we want this to be small double angle = //we want this to be small
Math.Acos((currentDistanceToGoal * currentDistanceToGoal + currentDistanceToNeighbor + double.RadiansToDegrees(
currentDistanceToNeighbor - neighborDistanceToGoal * neighborDistanceToGoal) / (2 * Math.Acos((currentDistanceToGoal * currentDistanceToGoal +
currentDistanceToGoal * currentDistanceToNeighbor));*/ currentDistanceToNeighbor * currentDistanceToNeighbor -
neighborDistanceToGoal * neighborDistanceToGoal) /
(2 * currentDistanceToGoal * currentDistanceToNeighbor)));
double distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 100; //double distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 100;
//double angleRating = Math.Abs((180 - angle) / 180) * 100; double angleRating = 100 - (angle < 180 ? angle / 180 : (360 - angle) / 180) * 100;
double speedRating = speed * 1.0 / _maxSpeed * 100; double speedRating = speed * 1.0 / _maxSpeed * 100;
//double distanceSpeedRating = ((totalDistance / _maxSpeed) / (neighborDistanceToGoal / speed)) * 100;
return 300 - (int)(distanceRating * 2 + speedRating * 1); return (int)-(speedRating + angleRating * 1.4);
} }
} }

3
astar/PathingHelper/SpeedHelper.cs
View File

@ -1,5 +1,4 @@
using Graph; using OSM_Graph.Enums;
using OSM_Graph.Enums;
namespace astar.PathingHelper; namespace astar.PathingHelper;