OSMServer/Pathfinding/Pathfinder_Distance.cs

51 lines
2.1 KiB
C#
Raw Normal View History

using OSMDatastructure;
using OSMDatastructure.Graph;
namespace Pathfinding;
public static partial class Pathfinder
{
public static List<PathNode> AStarDistance(string workingDir, Coordinates start,
Coordinates goal)
{
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)
{
2023-04-09 18:37:45 +02:00
OsmNode currentNode = toVisit.Dequeue();
2023-04-09 18:37:45 +02:00
foreach (OsmEdge edge in currentNode.edges)
{
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);
2023-04-09 18:37:45 +02:00
double newPotentialLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, neighbor);
2023-04-09 16:24:43 +02:00
if (newPotentialLength < neighbor.currentPathLength)
{
2023-04-09 18:37:45 +02:00
neighbor.previousPathNode = currentNode;
2023-04-09 16:24:43 +02:00
neighbor.currentPathLength = newPotentialLength;
if(neighbor.Equals(goalNode))
return GetRouteFromCalc(goalNode, regionManager);
//stop = true;
if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)) && !stop)
{
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
}
}
}
}
}
return GetRouteFromCalc(goalNode, regionManager);
}
}