OSMServer/Pathfinding/Pathfinder.cs
glax 58d1031524 Splitting Pathfinding into separate files for each type of routing.
Removing timetracking again from routing-algos (not in scope)
2023-04-09 16:17:15 +02:00

83 lines
3.2 KiB
C#

using OSMDatastructure;
using OSMDatastructure.Graph;
using OSMImporter;
namespace Pathfinding;
public static partial class Pathfinder
{
private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager )
{
ValueTuple<OsmNode?, OsmNode?> retTuple = new();
retTuple.Item1 = ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.road, regionManager);
retTuple.Item2 = ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.road, regionManager);
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;
}
public static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Tag.SpeedType vehicle, RegionManager regionManager)
{
OsmNode? closest = null;
double distance = double.MaxValue;
Region? region = regionManager.GetRegion(coordinates);
if (region is null)
return null;
foreach (OsmNode node in region.nodes)
{
bool hasConnectionUsingVehicle = false;
foreach (OsmEdge edge in node.edges)
{
double speed = GetSpeedForEdge(node, edge.wayId, vehicle, regionManager);
if (speed != 0)
hasConnectionUsingVehicle = true;
}
double nodeDistance = Utils.DistanceBetween(node, coordinates);
if (nodeDistance < distance && hasConnectionUsingVehicle)
{
closest = node;
distance = nodeDistance;
}
}
return closest;
}
private static double GetSpeedForEdge(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, RegionManager regionManager)
{
TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!;
switch (vehicle)
{
case Tag.SpeedType.pedestrian:
byte speed = Tag.defaultSpeedPedestrian[wayType];
if (speed is not 0)
return speed;
return 0;
case Tag.SpeedType.car:
case Tag.SpeedType.road:
byte? maxSpeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
if (maxSpeed is not null)
return (double)maxSpeed;
maxSpeed = Tag.defaultSpeedCar[wayType];
if(maxSpeed is not 0)
return (byte)maxSpeed;
return 0;
default:
return 0;
}
}
private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
{
double distance = Utils.DistanceBetween(node1, node2);
double speed = GetSpeedForEdge(node1, wayId, vehicle, regionManager);
if (speed is not 0)
return distance / speed;
return double.PositiveInfinity;
}
}