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

Created GetSpeed method for uniform implementation

Browse Source 
Changed Pathfinding class to static
Changed AStar to continue after path found.
This commit is contained in:
glax 2023-04-06 02:23:12 +02:00
parent 3e23635cd1
commit f42e458048
1 changed files with 32 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
Pathfinding/Pathfinder.cs
View File

@ -4,7 +4,7 @@ using OSMImporter;
namespace Pathfinding; namespace Pathfinding;
public class Pathfinder public static class Pathfinder
{ {
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle) public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
{ {
@ -14,8 +14,8 @@ 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, vehicle); OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle); OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager);
if (startNode == null || goalNode == null) if (startNode == null || goalNode == null)
return new List<OsmNode>(); return new List<OsmNode>();
@ -27,7 +27,7 @@ public class Pathfinder
OsmNode closestNodeToGoal = startNode; OsmNode closestNodeToGoal = startNode;
bool stop = false; bool stop = false;
while (toVisit.Count > 0 && !stop) while (toVisit.Count > 0)
{ {
closestNodeToGoal = toVisit.Dequeue(); closestNodeToGoal = toVisit.Dequeue();
Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}"); Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
@ -44,10 +44,13 @@ public class Pathfinder
neighbor.previousPathNode = closestNodeToGoal; neighbor.previousPathNode = closestNodeToGoal;
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) || closestNodeToGoal.directDistanceToGoal < 10) //change for faster if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10)
{
stop = true; stop = true;
else goalNode = neighbor;
}
else if(!stop)
{ {
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
@ -58,7 +61,7 @@ public class Pathfinder
} }
List<OsmNode> path = new(); List<OsmNode> path = new();
OsmNode? currentNode = closestNodeToGoal; OsmNode? currentNode = goalNode;
while (currentNode != null && !currentNode.Equals(startNode)) while (currentNode != null && !currentNode.Equals(startNode))
{ {
path.Add(currentNode); path.Add(currentNode);
@ -70,7 +73,7 @@ public class Pathfinder
return path; return path;
} }
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle) private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle, ref RegionManager regionManager)
{ {
OsmNode? closest = null; OsmNode? closest = null;
double distance = double.MaxValue; double distance = double.MaxValue;
@ -79,19 +82,7 @@ public class Pathfinder
bool hasConnection = false; bool hasConnection = false;
foreach (OsmEdge edge in node.edges) foreach (OsmEdge edge in node.edges)
{ {
byte speed = 0; double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager);
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) if (speed != 0)
hasConnection = true; hasConnection = true;
} }
@ -106,30 +97,38 @@ public class Pathfinder
return closest; return closest;
} }
private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager) private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
{ {
TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager; TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
double distance = Utils.DistanceBetween(node1, node2);
Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!; Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!;
switch (vehicle) switch (vehicle)
{ {
case Tag.SpeedType.pedestrian: case Tag.SpeedType.pedestrian:
byte speed = Tag.defaultSpeedPedestrian[wayType]; byte speed = Tag.defaultSpeedPedestrian[wayType];
if(speed is not 0) if (speed is not 0)
return distance / speed; return speed;
else return Double.PositiveInfinity; return 0;
case Tag.SpeedType.car: case Tag.SpeedType.car:
case Tag.SpeedType.road: case Tag.SpeedType.road:
byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed); byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
if (maxspeed is not null) if (maxspeed is not null)
return distance / Convert.ToDouble(maxspeed); return (double)maxspeed;
else maxspeed = Tag.defaultSpeedCar[wayType];
maxspeed = Tag.defaultSpeedCar[wayType];
if(maxspeed is not 0) if(maxspeed is not 0)
return distance / Tag.defaultSpeedCar[wayType]; return (byte)maxspeed;
else return Double.PositiveInfinity;; return 0;
default: default:
return double.PositiveInfinity; return 0;
} }
} }
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;
else
return double.PositiveInfinity;
}
} }