OSMServer/Pathfinding/Pathfinder.cs
glax c238a9eed3 Implemented "smart" routing through priority:
Factors:
speed,
roadtype,
junctions / waychanges.
2023-04-09 20:41:33 +02:00

166 lines
6.4 KiB
C#

using OSMDatastructure;
using OSMDatastructure.Graph;
using WayType = OSMDatastructure.Tag.WayType;
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 = 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;
}
private static double EdgeWeight(OsmNode node1, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager)
{
OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
if (node2 is null)
return double.MaxValue;
double distance = Utils.DistanceBetween(node1, node2);
byte speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle);
return speed is 0 ? double.MaxValue : distance / speed;
}
private static double GetPriority(OsmNode current, OsmNode? previous, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager)
{
if (vehicle == Tag.SpeedType.any)
return 1;
const double roadPriorityFactor = 1;
const double junctionFactor = 2;
const double wayChangeFactor = 2;
Region r = regionManager.GetRegion(current.coordinates)!;
double roadPriority = GetPriorityVehicleRoad(edge, vehicle, r) * 0.1 * roadPriorityFactor;
double roadSpeed = regionManager.GetSpeedForEdge(current, edge.wayId, vehicle);
if(vehicle == Tag.SpeedType.pedestrian)
return (current.directDistanceToGoal / roadSpeed) * roadPriority;
ulong previousWayId = UInt64.MaxValue;
if (previous?.edges is not null)
{
foreach (OsmEdge e in previous.edges)
if (e.neighborId.Equals(current.nodeId))
{
previousWayId = e.wayId;
break;
}
}
double junctionPriority = (current.edges.Count > 2 ? 1 : 0) * junctionFactor;
double wayChange = (previousWayId != edge.wayId ? 1 : 0) * wayChangeFactor;
if(vehicle == Tag.SpeedType.car)
return (current.directDistanceToGoal / roadSpeed) * roadPriority * ((junctionPriority + wayChange) / 2);
return double.MaxValue;
}
private static double GetPriorityVehicleRoad(OsmEdge edge, Tag.SpeedType vehicle, Region region)
{
if (vehicle == Tag.SpeedType.any)
return 1;
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway);
if(wayType is null)
return double.MaxValue;
if (vehicle == Tag.SpeedType.car)
{
switch (wayType)
{
case WayType.motorway:
case WayType.motorway_link:
case WayType.motorroad:
case WayType.trunk:
case WayType.trunk_link:
case WayType.primary:
case WayType.primary_link:
return 1;
case WayType.secondary:
case WayType.secondary_link:
return 2;
case WayType.tertiary:
case WayType.tertiary_link:
return 3;
case WayType.unclassified:
case WayType.residential:
case WayType.road:
return 4;
case WayType.living_street:
case WayType.service:
case WayType.track:
return 5;
default:
return 100;
}
}
if (vehicle == Tag.SpeedType.pedestrian)
{
switch (wayType)
{
case WayType.pedestrian:
case WayType.corridor:
case WayType.footway:
case WayType.path:
case WayType.steps:
case WayType.residential:
case WayType.living_street:
return 1;
case WayType.service:
case WayType.cycleway:
case WayType.bridleway:
case WayType.road:
case WayType.track:
case WayType.unclassified:
return 2;
case WayType.tertiary:
case WayType.tertiary_link:
case WayType.escape:
return 5;
default:
return 100;
}
}
return 100;
}
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;
}
}