30 lines
1.2 KiB
C#
30 lines
1.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 = regionManager.ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.road);
|
|
retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.road);
|
|
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, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
|
|
{
|
|
double distance = Utils.DistanceBetween(node1, node2);
|
|
double speed = regionManager.GetSpeedForEdge(node1, wayId, vehicle);
|
|
if (speed is not 0)
|
|
return distance / speed;
|
|
return double.PositiveInfinity;
|
|
}
|
|
} |