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

Keep searching for the same amount of time already search to get shorter path.

Browse Source
This commit is contained in:
glax 2023-04-23 14:50:16 +02:00
parent 5f6cccd17d
commit dca4d56866
1 changed files with 34 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
Pathfinding/Pathfinder.cs
View File

@ -54,17 +54,31 @@ public class Pathfinder
gScore = new() { { startNode, 0 } }; gScore = new() { { startNode, 0 } };
_cameFromDict = new(); _cameFromDict = new();
while (openSetfScore.Count > 0) bool found = false;
bool stop = false;
TimeSpan firstFound = TimeSpan.MaxValue;
double maxGscore = double.MaxValue;
while (openSetfScore.Count > 0 && !stop)
{ {
OsmNode currentNode = openSetfScore.Dequeue(); OsmNode currentNode = openSetfScore.Dequeue()!;
if (currentNode.Equals(goalNode)) if (currentNode.Equals(goalNode))
{ {
TimeSpan calcTime = DateTime.Now - startCalc; if (!found)
pathResult = GetPath(goalNode, calcTime); {
Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}"); firstFound = DateTime.Now - startCalc;
return this; found = true;
}
maxGscore = gScore[goalNode];
/*
int removed = openSetfScore.Remove(gScore.Where(item => item.Value > maxGscore)
.ToDictionary(item => item.Key, item => item.Value).Keys);
Console.WriteLine($"Removed {removed}");*/
} }
if (found && DateTime.Now - startCalc > firstFound.Multiply(2))
stop = true;
foreach (OsmEdge edge in currentNode.edges) foreach (OsmEdge edge in currentNode.edges)
{ {
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
@ -72,7 +86,7 @@ public class Pathfinder
{ {
double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge); double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge);
gScore.TryAdd(neighbor, double.MaxValue); gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor]) if ((!found || (found && tentativeGScore < maxGscore)) && tentativeGScore < gScore[neighbor])
{ {
if(!_cameFromDict.TryAdd(neighbor, currentNode)) if(!_cameFromDict.TryAdd(neighbor, currentNode))
_cameFromDict[neighbor] = currentNode; _cameFromDict[neighbor] = currentNode;
@ -83,8 +97,19 @@ public class Pathfinder
} }
} }
} }
pathResult = new(DateTime.Now - startCalc, new List<PathNode>(),0 ,0); TimeSpan calcTime = DateTime.Now - startCalc;
if (!found)
{
pathResult = new(DateTime.Now - startCalc, new List<PathNode>(),0 ,0);
Console.Write("No path found.");
return this;
}
else
{
pathResult = GetPath(goalNode, calcTime);
}
Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
return this; return this;
} }