OSMServer/Pathfinding/Pathfinder.cs

63 lines
2.7 KiB
C#
Raw Normal View History

2023-02-03 23:35:22 +01:00
using OSMDatastructure;
using OSMDatastructure.Graph;
2023-02-03 23:35:22 +01:00
namespace Pathfinding;
public static partial class Pathfinder
2023-02-03 23:35:22 +01:00
{
private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager )
{
ValueTuple<OsmNode?, OsmNode?> retTuple = new();
retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.any);
retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.any);
if (retTuple.Item1 is null || retTuple.Item2 is null)
return retTuple;
retTuple.Item1.currentPathWeight = 0;
retTuple.Item1.currentPathLength = 0;
retTuple.Item1.directDistanceToGoal = Utils.DistanceBetween(retTuple.Item1, retTuple.Item2);
return retTuple;
2023-02-03 23:35:22 +01:00
}
2023-04-09 17:00:28 +02:00
private static double EdgeWeight(OsmNode node1, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager)
{
2023-04-09 17:00:28 +02:00
OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
if (node2 is null)
return double.MaxValue;
double distance = Utils.DistanceBetween(node1, node2);
2023-04-09 17:00:28 +02:00
double speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle);
if (speed is 0)
return double.MaxValue;
return distance / speed;
}
private static List<PathNode> GetRouteFromCalc(OsmNode goalNode, RegionManager regionManager)
{
List<PathNode> path = new();
OsmNode? currentNode = goalNode;
while (currentNode is not null)
{
HashSet<Tag>? tags = null;
double pathDistanceDelta = 0;
double pathWeightDelta = 0;
double directDistanceDelta = 0;
if (currentNode.previousPathNode is not null)
{
OsmEdge edge = currentNode.previousPathNode!.edges.First(e => e.neighborId.Equals(currentNode.nodeId));
tags = regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(edge.wayId);
pathDistanceDelta = currentNode.currentPathLength - currentNode.previousPathNode.currentPathLength;
pathWeightDelta = currentNode.currentPathWeight - currentNode.previousPathNode.currentPathWeight;
directDistanceDelta =
currentNode.directDistanceToGoal - currentNode.previousPathNode.directDistanceToGoal;
}
PathNode? pn = PathNode.FromOsmNode(currentNode, tags, pathDistanceDelta, pathWeightDelta, directDistanceDelta);
if(pn is not null)
path.Add(pn!);
currentNode = currentNode.previousPathNode;
}
path.Reverse();
return path;
}
2023-02-03 23:35:22 +01:00
}