82 lines
3.2 KiB
C#
82 lines
3.2 KiB
C#
using OSMDatastructure;
|
|
using OSMDatastructure.Graph;
|
|
|
|
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;
|
|
}
|
|
} |