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

Replaced local vars with fields.

Browse Source
This commit is contained in:
glax 2023-04-20 22:58:27 +02:00
parent 90a09e84c5
commit 2bd6c5d9c4
1 changed files with 19 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
Pathfinding/Pathfinder.cs
View File

@ -6,7 +6,6 @@ using WayType = OSMDatastructure.Tag.WayType;
namespace Pathfinding;
//TODO check parameters for all functions and determine global fields
public class Pathfinder
{
@ -14,6 +13,8 @@ public class Pathfinder
public readonly string workingDir;
public PathResult? pathResult;
public Dictionary<OsmNode, double>? gScore;
private Dictionary<OsmNode, OsmNode>? cameFromDict;
private SpeedType _speedType;
public Pathfinder(string workingDirectory)
{
@ -29,8 +30,9 @@ public class Pathfinder
{
DateTime startCalc = DateTime.Now;
regionManager = new RegionManager(workingDir);
OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle);
_speedType = vehicle;
OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, _speedType);
OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, _speedType);
if (startNode is null || goalNode is null)
{
pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
@ -39,8 +41,8 @@ public class Pathfinder
PriorityQueue<OsmNode, double> openSetfScore = new();
openSetfScore.Enqueue(startNode, 0);
Dictionary<OsmNode, OsmNode> cameFromDict = new();
gScore = new() { { startNode, 0 } };
cameFromDict = new();
while (openSetfScore.Count > 0)
{
@ -48,7 +50,7 @@ public class Pathfinder
if (currentNode.Equals(goalNode))
{
Console.WriteLine("Path found.");
this.pathResult = GetPath(cameFromDict, goalNode, DateTime.Now - startCalc);
this.pathResult = GetPath(goalNode, DateTime.Now - startCalc);
return this;
}
@ -58,14 +60,14 @@ public class Pathfinder
if (neighbor is not null)
{
double tentativeGScore =
gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle);
gScore[currentNode] + Weight(currentNode, neighbor, edge);
gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor])
{
if(!cameFromDict.TryAdd(neighbor, currentNode))
cameFromDict[neighbor] = currentNode;
gScore[neighbor] = tentativeGScore;
double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle,
double h = Heuristic(currentNode, neighbor, goalNode, edge,
heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority);
//Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}");
openSetfScore.Enqueue(neighbor, tentativeGScore + h);
@ -86,11 +88,11 @@ public class Pathfinder
Console.WriteLine($"Saved result to {path}");
}
private PathResult GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, TimeSpan calcFinished)
private PathResult GetPath(OsmNode goalNode, TimeSpan calcFinished)
{
List<PathNode> path = new();
OsmNode currentNode = goalNode;
while (cameFromDict.ContainsKey(cameFromDict[currentNode]))
while (cameFromDict!.ContainsKey(cameFromDict[currentNode]))
{
OsmEdge? currentEdge = cameFromDict[currentNode].edges.First(edge => edge.neighborId == currentNode.nodeId);
HashSet<Tag>? tags =
@ -106,17 +108,17 @@ public class Pathfinder
return new PathResult(calcFinished, path);
}
private double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle)
private double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge)
{
double distance = Utils.DistanceBetween(fromNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle);
double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, _speedType);
//double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
return distance / speed;
}
private double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
private double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
{
double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
TagManager curTags = regionManager.GetRegion(fromNode.coordinates)!.tagManager;
TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
@ -140,14 +142,14 @@ public class Pathfinder
return Utils.DistanceBetween(neighborNode, goalNode) / (1 + roadPriority + sameRoadName + junctionCount);
}
private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region)
private double GetPriorityVehicleRoad(OsmEdge edge, Region region)
{
if (vehicle == SpeedType.any)
if (_speedType == SpeedType.any)
return 1;
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
if(wayType is null)
return 0;
if (vehicle == SpeedType.car)
if (_speedType == SpeedType.car)
{
switch (wayType)
{
@ -175,7 +177,7 @@ public class Pathfinder
return 0.0001;
}
}
if (vehicle == SpeedType.pedestrian)
if (_speedType == SpeedType.pedestrian)
{
switch (wayType)
{