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;
public class Pathfinder
public static class Pathfinder
{
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)
return new List<OsmNode>();
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle);
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager);
if (startNode == null || goalNode == null)
return new List<OsmNode>();
@ -27,7 +27,7 @@ public class Pathfinder
OsmNode closestNodeToGoal = startNode;
bool stop = false;
while (toVisit.Count > 0 && !stop)
while (toVisit.Count > 0)
{
closestNodeToGoal = toVisit.Dequeue();
Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
@ -44,10 +44,13 @@ public class Pathfinder
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = newPotentialWeight;
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;
else
goalNode = neighbor;
}
else if(!stop)
{
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
@ -58,7 +61,7 @@ public class Pathfinder
}
List<OsmNode> path = new();
OsmNode? currentNode = closestNodeToGoal;
OsmNode? currentNode = goalNode;
while (currentNode != null && !currentNode.Equals(startNode))
{
path.Add(currentNode);
@ -70,7 +73,7 @@ public class Pathfinder
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;
double distance = double.MaxValue;
@ -79,19 +82,7 @@ public class Pathfinder
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;
}
double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager);
if (speed != 0)
hasConnection = true;
}
@ -106,30 +97,38 @@ public class Pathfinder
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;
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;
if (speed is not 0)
return speed;
return 0;
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];
return (double)maxspeed;
maxspeed = Tag.defaultSpeedCar[wayType];
if(maxspeed is not 0)
return distance / Tag.defaultSpeedCar[wayType];
else return Double.PositiveInfinity;;
return (byte)maxspeed;
return 0;
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;
}
}