63 lines
2.8 KiB
C#
63 lines
2.8 KiB
C#
using OSMDatastructure;
|
|
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, vehicle);
|
|
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);
|
|
|
|
while (toVisit.Count > 0)
|
|
{
|
|
OsmNode currentNode = toVisit.Dequeue();
|
|
|
|
foreach (OsmEdge edge in currentNode.edges.Where(
|
|
edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle)))
|
|
{
|
|
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
|
if (neighbor is not null)
|
|
{
|
|
if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1)
|
|
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
|
|
|
|
double newPotentialWeight = currentNode.currentPathWeight +
|
|
EdgeWeight(currentNode, edge, vehicle, regionManager);
|
|
|
|
if (newPotentialWeight < neighbor.currentPathWeight)
|
|
{
|
|
neighbor.previousPathNode = currentNode;
|
|
neighbor.currentPathWeight = newPotentialWeight;
|
|
|
|
if (neighbor.Equals(goalNode))
|
|
{
|
|
currentNode = neighbor;
|
|
currentNode.currentPathLength = 0;
|
|
while (!currentNode.Equals(startAndEndNode.Item1) && currentNode.previousPathNode is not null)
|
|
{
|
|
currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength +
|
|
Utils.DistanceBetween(currentNode, currentNode.previousPathNode);
|
|
currentNode = currentNode.previousPathNode;
|
|
}
|
|
|
|
return GetRouteFromCalc(goalNode, regionManager);
|
|
}
|
|
|
|
toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return GetRouteFromCalc(goalNode, regionManager);
|
|
}
|
|
} |