glax/OSMServer
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 } };
_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))
{
TimeSpan calcTime = DateTime.Now - startCalc;
pathResult = GetPath(goalNode, calcTime);
Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
return this;
if (!found)
{
firstFound = DateTime.Now - startCalc;
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)
{
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
@ -72,7 +86,7 @@ public class Pathfinder
{
double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge);
gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor])
if ((!found || (found && tentativeGScore < maxGscore)) && tentativeGScore < gScore[neighbor])
{
if(!_cameFromDict.TryAdd(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;
}