glax/OSMServer
Archived
1
0
Fork 0
Code Pull Requests Releases Wiki Activity

Changed ClosestNodeToCoordinates to include only nodes that have connections for appropriate SpeedType (e.g. roads for cars, footways for pedestrians)

Browse Source 
Changed toVisit to be a priorityqueue.

Search is aborted, if within 250m of goal.
This commit is contained in:
glax 2023-04-01 18:10:21 +02:00
parent 0f0f4182ac
commit 4600105b0b
1 changed files with 38 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
Pathfinding/Pathfinder.cs
View File

@ -14,27 +14,23 @@ public class Pathfinder
if (startRegion is null || goalRegion is null) if (startRegion is null || goalRegion is null)
return new List<OsmNode>(); return new List<OsmNode>();
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion); OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion); OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle);
if (startNode == null || goalNode == null) if (startNode == null || goalNode == null)
return new List<OsmNode>(); return new List<OsmNode>();
List<OsmNode> toVisit = new() { startNode }; PriorityQueue<OsmNode, double> toVisit = new();
OsmNode closestNodeToGoal = toVisit.First(); toVisit.Enqueue(startNode, 0);
closestNodeToGoal.currentPathWeight = 0; startNode.currentPathWeight = 0;
closestNodeToGoal.currentPathLength = 0; startNode.currentPathLength = 0;
startNode.directDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
OsmNode closestNodeToGoal = startNode;
bool stop = false; bool stop = false;
while (toVisit.Count > 0 && !stop) while (toVisit.Count > 0 && !stop)
{ {
//Console.WriteLine("toVisit-length: {0}", toVisit.Count); closestNodeToGoal = toVisit.Dequeue();
closestNodeToGoal = toVisit.First(); Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
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) foreach (OsmEdge edge in closestNodeToGoal.edges)
{ {
@ -49,19 +45,20 @@ public class Pathfinder
neighbor.currentPathWeight = newPotentialWeight; neighbor.currentPathWeight = newPotentialWeight;
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor); neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
if (neighbor.Equals(goalNode)) if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 250)
stop = true; stop = true;
else else
toVisit.Add(neighbor); {
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
}
} }
} }
} }
toVisit.Remove(closestNodeToGoal);
} }
List<OsmNode> path = new(); List<OsmNode> path = new();
OsmNode? currentNode = goalNode; OsmNode? currentNode = closestNodeToGoal;
while (currentNode != null && !currentNode.Equals(startNode)) while (currentNode != null && !currentNode.Equals(startNode))
{ {
path.Add(currentNode); path.Add(currentNode);
@ -73,14 +70,33 @@ public class Pathfinder
return path; return path;
} }
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region) private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle)
{ {
OsmNode? closest = null; OsmNode? closest = null;
double distance = double.MaxValue; double distance = double.MaxValue;
foreach (OsmNode node in region.nodes) foreach (OsmNode node in region.nodes)
{ {
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;
}
double nodeDistance = Utils.DistanceBetween(node, coordinates); double nodeDistance = Utils.DistanceBetween(node, coordinates);
if (nodeDistance < distance) if (nodeDistance < distance && hasConnection)
{ {
closest = node; closest = node;
distance = nodeDistance; distance = nodeDistance;