OSMServer/Pathfinding/Pathfinder_Time.cs

55 lines
2.3 KiB
C#
Raw Normal View History

2023-04-09 17:06:45 +02:00
using OSMDatastructure;
2023-04-09 16:47:30 +02:00
using OSMDatastructure.Graph;
using Utils = OSMDatastructure.Utils;
namespace Pathfinding;
public static partial class Pathfinder
{
public static List<PathNode> AStarTime(string workingDir, Coordinates start,
Coordinates goal, Tag.SpeedType vehicle)
{
RegionManager regionManager = new (workingDir);
ValueTuple<OsmNode?, OsmNode?> startAndEndNode = SetupNodes(start, goal, regionManager);
if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null)
return new List<PathNode>();
OsmNode goalNode = startAndEndNode.Item2!;
PriorityQueue<OsmNode, double> toVisit = new();
toVisit.Enqueue(startAndEndNode.Item1, 0);
bool stop = false;
while (toVisit.Count > 0)
{
OsmNode closestNodeToGoal = toVisit.Dequeue();
foreach (OsmEdge edge in closestNodeToGoal.edges.Where(
edge => regionManager.TestValidConnectionForType(closestNodeToGoal, edge, vehicle)))
{
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
if (neighbor is not null)
{
double newPotentialWeight = closestNodeToGoal.currentPathWeight +
2023-04-09 17:00:28 +02:00
EdgeWeight(closestNodeToGoal, edge, vehicle, regionManager);
2023-04-09 16:47:30 +02:00
if (newPotentialWeight < neighbor.currentPathWeight)
{
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
neighbor.currentPathWeight = newPotentialWeight;
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
if (neighbor.Equals(goalNode))
{
stop = true;
}
else if(!stop)
{
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
}
}
}
}
}
return GetRouteFromCalc(goalNode, regionManager);
2023-04-09 16:47:30 +02:00
}
}