119 lines
4.6 KiB
C#
119 lines
4.6 KiB
C#
using OSMDatastructure;
|
|
using OSMDatastructure.Graph;
|
|
using OSMImporter;
|
|
|
|
namespace Pathfinding;
|
|
|
|
public class Pathfinder
|
|
{
|
|
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
|
|
{
|
|
RegionManager regionManager = new RegionManager(workingDir);
|
|
Region? startRegion = regionManager.GetRegion(start);
|
|
Region? goalRegion = regionManager.GetRegion(goal);
|
|
if (startRegion is null || goalRegion is null)
|
|
return new List<OsmNode>();
|
|
|
|
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion);
|
|
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion);
|
|
if (startNode == null || goalNode == null)
|
|
return new List<OsmNode>();
|
|
|
|
List<OsmNode> toVisit = new() { startNode };
|
|
OsmNode closestNodeToGoal = toVisit.First();
|
|
closestNodeToGoal.currentPathWeight = 0;
|
|
closestNodeToGoal.currentPathLength = 0;
|
|
bool stop = false;
|
|
|
|
while (toVisit.Count > 0 && !stop)
|
|
{
|
|
//Console.WriteLine("toVisit-length: {0}", toVisit.Count);
|
|
closestNodeToGoal = toVisit.First();
|
|
foreach (OsmNode node in toVisit.Where(node => node.directDistanceToGoal.Equals(Double.MaxValue)))
|
|
{
|
|
node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode);
|
|
}
|
|
|
|
closestNodeToGoal = toVisit.OrderBy(node => node.directDistanceToGoal).First();
|
|
|
|
foreach (OsmEdge edge in closestNodeToGoal.edges)
|
|
{
|
|
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
|
if (neighbor is not null)
|
|
{
|
|
double newPotentialWeight =
|
|
closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager);
|
|
if (neighbor.currentPathWeight > newPotentialWeight)
|
|
{
|
|
neighbor.previousPathNode = closestNodeToGoal;
|
|
neighbor.currentPathWeight = newPotentialWeight;
|
|
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
|
|
|
|
if (neighbor.Equals(goalNode))
|
|
stop = true;
|
|
else
|
|
toVisit.Add(neighbor);
|
|
}
|
|
}
|
|
}
|
|
|
|
toVisit.Remove(closestNodeToGoal);
|
|
}
|
|
|
|
List<OsmNode> path = new();
|
|
OsmNode? currentNode = goalNode;
|
|
while (currentNode != null && !currentNode.Equals(startNode))
|
|
{
|
|
path.Add(currentNode);
|
|
currentNode = currentNode.previousPathNode;
|
|
}
|
|
path.Add(startNode);
|
|
path.Reverse();
|
|
|
|
return path;
|
|
}
|
|
|
|
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
|
|
{
|
|
OsmNode? closest = null;
|
|
double distance = double.MaxValue;
|
|
foreach (OsmNode node in region.nodes)
|
|
{
|
|
double nodeDistance = Utils.DistanceBetween(node, coordinates);
|
|
if (nodeDistance < distance)
|
|
{
|
|
closest = node;
|
|
distance = nodeDistance;
|
|
}
|
|
}
|
|
|
|
return closest;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
} |