101 lines
3.4 KiB
C#
101 lines
3.4 KiB
C#
using OSMDatastructure;
|
|
using OSMImporter;
|
|
|
|
namespace Pathfinding;
|
|
|
|
public class Pathfinder
|
|
{
|
|
|
|
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, OsmEdge.speedType vehicle)
|
|
{
|
|
RegionManager regionManager = new RegionManager(workingDir);
|
|
Region startRegion, goalRegion;
|
|
try
|
|
{
|
|
startRegion = regionManager.GetRegion(start);
|
|
goalRegion = regionManager.GetRegion(goal);
|
|
}
|
|
catch (FileNotFoundException e)
|
|
{
|
|
throw new Exception(string.Format("No region at coordinates {0}", e.FileName), e);
|
|
}
|
|
|
|
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;
|
|
bool stop = false;
|
|
|
|
while (toVisit.Count > 0 && !stop)
|
|
{
|
|
//Console.WriteLine("toVisit-length: {0}", toVisit.Count);
|
|
closestNodeToGoal = toVisit.First();
|
|
foreach (OsmNode node in toVisit)
|
|
{
|
|
if (node.directDistanceToGoal.Equals(double.MaxValue))
|
|
{
|
|
node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode);
|
|
}
|
|
if (node.directDistanceToGoal < closestNodeToGoal.directDistanceToGoal)
|
|
{
|
|
closestNodeToGoal = node;
|
|
}
|
|
}
|
|
|
|
foreach (OsmEdge edge in closestNodeToGoal.edges)
|
|
{
|
|
OsmNode? neighbor = regionManager.GetNode(edge.neighborCoordinates);
|
|
if (neighbor != null)
|
|
{
|
|
double newPotentialWeight =
|
|
closestNodeToGoal.currentPathWeight + edge.GetWeight(closestNodeToGoal, vehicle);
|
|
if (neighbor.currentPathWeight > newPotentialWeight)
|
|
{
|
|
neighbor.previousPathNode = closestNodeToGoal;
|
|
neighbor.currentPathWeight = newPotentialWeight;
|
|
|
|
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;
|
|
}
|
|
} |