OSMServer/Pathfinding/Pathfinder.cs

133 lines
5.3 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
using OSMImporter;
namespace Pathfinding;
public static class Pathfinder
2023-02-03 23:35:22 +01:00
{
2023-04-01 14:43:05 +02:00
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
2023-02-03 23:35:22 +01:00
{
RegionManager regionManager = new RegionManager(workingDir);
2023-04-01 14:43:05 +02:00
Region? startRegion = regionManager.GetRegion(start);
Region? goalRegion = regionManager.GetRegion(goal);
if (startRegion is null || goalRegion is null)
return new List<OsmNode>();
2023-02-06 17:32:55 +01:00
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager);
2023-02-06 17:32:55 +01:00
if (startNode == null || goalNode == null)
return new List<OsmNode>();
PriorityQueue<OsmNode, double> toVisit = new();
toVisit.Enqueue(startNode, 0);
startNode.currentPathWeight = 0;
startNode.currentPathLength = 0;
startNode.directDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
OsmNode closestNodeToGoal = startNode;
2023-02-05 20:03:47 +01:00
bool stop = false;
while (toVisit.Count > 0)
2023-02-05 20:03:47 +01:00
{
closestNodeToGoal = toVisit.Dequeue();
Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
2023-04-01 14:43:05 +02:00
foreach (OsmEdge edge in closestNodeToGoal.edges)
2023-02-05 20:03:47 +01:00
{
2023-04-01 14:43:05 +02:00
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
if (neighbor is not null)
2023-02-05 20:03:47 +01:00
{
double newPotentialWeight =
2023-04-01 14:43:05 +02:00
closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager);
2023-04-06 02:32:04 +02:00
if (newPotentialWeight < neighbor.currentPathWeight)
{
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = newPotentialWeight;
2023-02-08 19:09:46 +01:00
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10)
{
stop = true;
goalNode = neighbor;
}
else if(!stop)
{
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
}
}
2023-02-05 20:03:47 +01:00
}
}
}
2023-02-06 17:32:55 +01:00
List<OsmNode> path = new();
OsmNode? currentNode = goalNode;
2023-02-06 17:32:55 +01:00
while (currentNode != null && !currentNode.Equals(startNode))
2023-02-05 20:03:47 +01:00
{
path.Add(currentNode);
2023-02-06 17:32:55 +01:00
currentNode = currentNode.previousPathNode;
2023-02-05 20:03:47 +01:00
}
path.Add(startNode);
path.Reverse();
2023-02-05 20:03:47 +01:00
return path;
2023-02-03 23:35:22 +01:00
}
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle, ref RegionManager regionManager)
2023-02-03 23:35:22 +01:00
{
2023-02-06 17:32:55 +01:00
OsmNode? closest = null;
double distance = double.MaxValue;
foreach (OsmNode node in region.nodes)
2023-02-03 23:35:22 +01:00
{
2023-04-06 02:32:04 +02:00
bool hasConnectionUsingVehicle = false;
foreach (OsmEdge edge in node.edges)
{
double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager);
if (speed != 0)
2023-04-06 02:32:04 +02:00
hasConnectionUsingVehicle = true;
}
2023-02-06 17:32:55 +01:00
double nodeDistance = Utils.DistanceBetween(node, coordinates);
2023-04-06 02:32:04 +02:00
if (nodeDistance < distance && hasConnectionUsingVehicle)
2023-02-03 23:35:22 +01:00
{
2023-02-06 17:32:55 +01:00
closest = node;
distance = nodeDistance;
2023-02-03 23:35:22 +01:00
}
}
2023-02-06 17:32:55 +01:00
return closest;
2023-04-01 14:43:05 +02:00
}
private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
2023-04-01 14:43:05 +02:00
{
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;
2023-04-01 14:43:05 +02:00
case Tag.SpeedType.car:
case Tag.SpeedType.road:
2023-04-06 02:32:04 +02:00
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;
2023-04-01 14:43:05 +02:00
default:
return 0;
2023-04-01 14:43:05 +02:00
}
}
private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
{
double distance = Utils.DistanceBetween(node1, node2);
double speed = GetSpeed(node1, wayId, vehicle, ref regionManager);
if (speed is not 0)
return distance / speed;
2023-04-06 02:32:04 +02:00
return double.PositiveInfinity;
}
2023-02-03 23:35:22 +01:00
}