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

Working State

Browse Source
This commit is contained in:
glax 2024-07-23 17:07:31 +02:00
parent 0758ae1d86
commit 832bc84bd2
3 changed files with 21 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
astar/Astar.cs
View File

@ -64,7 +64,7 @@ namespace astar
Node neighborNode = graph.Nodes[neighborId]; Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End
return PathFound(graph, currentNodeStart, neighborNode, logger); return PathFound(graph, currentNodeStart, neighborNode, car, logger);
float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode); float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance) if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
@ -102,7 +102,7 @@ namespace astar
Node neighborNode = graph.Nodes[neighborId]; Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End
return PathFound(graph, neighborNode, currentNodeEnd, logger); return PathFound(graph, neighborNode, currentNodeEnd, car, logger);
float distance = (currentNodeEnd.Distance??float.MaxValue) + (float)currentNodeEnd.DistanceTo(neighborNode); float distance = (currentNodeEnd.Distance??float.MaxValue) + (float)currentNodeEnd.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance) if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
@ -120,25 +120,30 @@ namespace astar
return new Route(graph, Array.Empty<Step>().ToList(), false); return new Route(graph, Array.Empty<Step>().ToList(), false);
} }
private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, 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!");
List<Step> path = new(); List<Step> path = new();
path.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, fromEnd)); 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)));
Node current = fromStart; Node current = fromStart;
while (current.Distance != 0f) while (current.Distance != 0f)
{ {
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[(ulong)current.PreviousNodeId!], current), graph.Nodes[(ulong)current.PreviousNodeId!], current); Node previous = graph.Nodes[(ulong)current.PreviousNodeId!];
OSM_Graph.Way previousToCurrent = graph.Ways[previous.Neighbors.First(n => graph.Nodes[n.Key] == current).Value.Key];
Step step = new(previous, current, (float)previous.DistanceTo(current), SpeedHelper.GetSpeed(previousToCurrent, car));
path.Add(step); path.Add(step);
current = graph.Nodes[(ulong)current.PreviousNodeId!]; current = previous;
} }
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.Distance != 0f)
{ {
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[(ulong)current.PreviousNodeId!], current), current, 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];
Step step = new(current, next, (float)current.DistanceTo(next), SpeedHelper.GetSpeed(currentToNext, car));
path.Add(step); path.Add(step);
current = graph.Nodes[(ulong)current.PreviousNodeId!]; current = next;
} }
Route r = new (graph, path, true); Route r = new (graph, path, true);

2
astar/PathingHelper/PriorityHelper.cs
View File

@ -20,6 +20,6 @@ public class PriorityHelper(double totalDistance, byte maxSpeed)
//double angleRating = Math.Abs((180 - angle) / 180) * 100; //double angleRating = Math.Abs((180 - angle) / 180) * 100;
double speedRating = speed * 1.0 / _maxSpeed * 100; double speedRating = speed * 1.0 / _maxSpeed * 100;
return 350 - (int)(distanceRating * 2 + speedRating * 1.5); return 300 - (int)(distanceRating * 2 + speedRating * 1);
} }
} }

10
astar/Route.cs
View File

@ -7,6 +7,8 @@
public bool RouteFound { get; } = routeFound; public bool RouteFound { get; } = routeFound;
public float Distance => Steps.Sum(step => step.Distance); public float Distance => Steps.Sum(step => step.Distance);
public TimeSpan Time => TimeSpan.FromHours(Steps.Sum(step => step.Distance / 1000 / step.Speed));
public ValueTuple<float, float> MinCoordinates() public ValueTuple<float, float> MinCoordinates()
{ {
float minLat = Graph.Nodes.MinBy(node => node.Value.Lat).Value.Lat; float minLat = Graph.Nodes.MinBy(node => node.Value.Lat).Value.Lat;
@ -24,18 +26,20 @@
public override string ToString() public override string ToString()
{ {
return $"{string.Join("\n", Steps)}\n" + return $"{string.Join("\n", Steps)}\n" +
$"Distance: {Distance:000000.00}m"; $"Distance: {Distance:000000.00}m\n" +
$"Time: {Time:000000.00}";
} }
} }
public struct Step(float distance, Node node1, Node node2) public struct Step(Node node1, Node node2, float distance, byte speed)
{ {
public readonly Node Node1 = node1, Node2 = node2; public readonly Node Node1 = node1, Node2 = node2;
public readonly float Distance = distance; public readonly float Distance = distance;
public readonly byte Speed = speed;
public override string ToString() public override string ToString()
{ {
return $"{Node1.Lat:00.000000} {Node1.Lon:000.000000} --- {Distance:0000.00}m ---> {Node2.Lat:00.000000} {Node2.Lon:000.000000}"; return $"{Node1.Lat:00.000000} {Node1.Lon:000.000000} --- {Distance:0000.00}m {Speed:000} ---> {Node2.Lat:00.000000} {Node2.Lon:000.000000}";
} }
} }
} }