glax/AStar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed Comparer (was sorting largest to smallest),

Browse Source 
Added second check to ensure shortest path
This commit is contained in:
C9Glax 2022-05-11 22:17:44 +02:00
parent e212aeef0d
commit 97d3cc8af9
1 changed files with 36 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
astar/Astar.cs
View File

@ -17,7 +17,7 @@ namespace astar
{ {
Node n1 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))]; Node n1 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
Node n2 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))]; Node n2 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3}", n1.lat, n1.lon, n2.lat, n2.lon); logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", n1.lat, n1.lon, n2.lat, n2.lon, Utils.DistanceBetweenNodes(n1,n2));
path = FindPath(ref nodes, n1, n2, ref logger); path = FindPath(ref nodes, n1, n2, ref logger);
} }
@ -41,7 +41,7 @@ namespace astar
/* /*
* *
*/ */
private static List<Node> FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, ref Logger ?logger) private static List<Node> FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, ref Logger? logger)
{ {
Reset(ref nodes); Reset(ref nodes);
List<Node> toVisit = new(); List<Node> toVisit = new();
@ -49,7 +49,7 @@ namespace astar
Node currentNode = start; Node currentNode = start;
start.pathLength = 0; start.pathLength = 0;
start.goalDistance = Utils.DistanceBetweenNodes(start, goal); start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
while(currentNode != goal && toVisit.Count > 0) while (currentNode != goal && toVisit.Count > 0)
{ {
currentNode = toVisit.First(); currentNode = toVisit.First();
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance); logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
@ -71,7 +71,37 @@ namespace astar
List<Node> path = new(); List<Node> path = new();
if (currentNode != goal) if (currentNode == goal)
{
if(toVisit[0].pathLength < goal.pathLength)
{
logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
while (toVisit[0].pathLength < goal.pathLength)
{
currentNode = toVisit.First();
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
//Check all neighbors of current node
foreach (Edge e in currentNode.edges)
{
if (e.neighbor.goalDistance == double.MaxValue)
e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
if (e.neighbor.pathLength > currentNode.pathLength + e.weight)
{
e.neighbor.pathLength = currentNode.pathLength + e.weight;
e.neighbor.previousNode = currentNode;
toVisit.Add(e.neighbor);
}
}
toVisit.Remove(currentNode); //"Mark" as visited
toVisit.Sort(CompareDistanceToGoal);
}
}
else
{
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
}
}
else
{ {
logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon); logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon);
return path; return path;
@ -97,9 +127,9 @@ namespace astar
else else
{ {
if (n1.goalDistance < n2.goalDistance) if (n1.goalDistance < n2.goalDistance)
return 1;
else if (n1.goalDistance > n2.goalDistance)
return -1; return -1;
else if (n1.goalDistance > n2.goalDistance)
return 1;
else return 0; else return 0;
} }
} }