OSMServer/Pathfinding/Pathfinder.cs

135 lines
5.5 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 class Pathfinder
{
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);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle);
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 && !stop)
{
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);
if (neighbor.currentPathWeight > newPotentialWeight)
{
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = newPotentialWeight;
2023-02-08 19:09:46 +01:00
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
2023-02-05 20:03:47 +01:00
2023-04-01 18:27:52 +02:00
if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10) //change for faster
stop = true;
else
{
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 = closestNodeToGoal;
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)
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
{
bool hasConnection = false;
foreach (OsmEdge edge in node.edges)
{
byte speed = 0;
switch (vehicle)
{
case Tag.SpeedType.road:
case Tag.SpeedType.car:
speed = Tag.defaultSpeedCar[(Tag.WayType)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway)!];
break;
case Tag.SpeedType.pedestrian:
speed = Tag.defaultSpeedPedestrian[
(Tag.WayType)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway)!];
break;
}
if (speed != 0)
hasConnection = true;
}
2023-02-06 17:32:55 +01:00
double nodeDistance = Utils.DistanceBetween(node, coordinates);
if (nodeDistance < distance && hasConnection)
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 EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
{
TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
double distance = Utils.DistanceBetween(node1, node2);
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 distance / speed;
else return Double.PositiveInfinity;
case Tag.SpeedType.car:
case Tag.SpeedType.road:
byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
if (maxspeed is not null)
return distance / Convert.ToDouble(maxspeed);
else
maxspeed = Tag.defaultSpeedCar[wayType];
if(maxspeed is not 0)
return distance / Tag.defaultSpeedCar[wayType];
else return Double.PositiveInfinity;;
default:
return double.PositiveInfinity;
}
}
2023-02-03 23:35:22 +01:00
}