AStar/astar/Astar.cs

138 lines
4.7 KiB
C#
Raw Normal View History

using Logging;
using Graph;
2022-05-05 02:00:55 +02:00
namespace astar
{
public class Astar
{
/*
* Resets the calculated previous nodes and distance to goal
*/
2022-05-06 00:02:28 +02:00
private static void Reset(ref Dictionary<ulong, Node> nodes)
{
foreach(Node n in nodes.Values)
{
2022-11-02 18:07:22 +01:00
n.previousNode = null;
2022-10-31 23:10:28 +01:00
n.goalDistance = float.MaxValue;
2022-11-02 18:09:30 +01:00
n.timeSpent = float.MaxValue;
2022-05-06 00:02:28 +02:00
}
}
/*
*
*/
2022-11-01 05:08:09 +01:00
public static bool FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, out List<Node> path, Logger? logger)
2022-05-06 00:02:28 +02:00
{
2022-11-01 05:08:09 +01:00
path = new List<Node>();
logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal));
Reset(ref nodes);
2022-05-11 20:25:13 +02:00
List<Node> toVisit = new();
2022-05-06 00:02:28 +02:00
toVisit.Add(start);
Node currentNode = start;
2022-11-02 18:09:30 +01:00
start.timeSpent = 0;
start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
2022-11-02 18:09:30 +01:00
while (toVisit.Count > 0 && toVisit[0].timeSpent < goal.timeSpent)
2022-05-06 00:02:28 +02:00
{
2022-11-01 05:08:09 +01:00
if(currentNode == goal)
{
logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
}
currentNode = toVisit.First();
2022-11-02 18:09:30 +01:00
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.timeSpent, currentNode.goalDistance);
//Check all neighbors of current node
2022-05-06 00:02:28 +02:00
foreach (Edge e in currentNode.edges)
{
2022-11-02 18:09:30 +01:00
if (e.neighbor.timeSpent > currentNode.timeSpent + e.weight)
{
2022-11-01 05:08:09 +01:00
e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
2022-11-02 18:09:30 +01:00
e.neighbor.timeSpent = currentNode.timeSpent + e.weight;
2022-05-06 00:02:28 +02:00
e.neighbor.previousNode = currentNode;
toVisit.Add(e.neighbor);
2022-05-06 00:02:28 +02:00
}
}
2022-11-01 05:08:09 +01:00
toVisit.Remove(currentNode); //"Mark" as visited
2022-11-01 05:08:09 +01:00
toVisit.Sort(CompareDistance);
2022-05-06 00:02:28 +02:00
}
2022-11-02 18:07:22 +01:00
if(goal.previousNode != null)
{
2022-11-01 05:08:09 +01:00
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
}
else
2022-05-06 00:02:28 +02:00
{
logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon);
2022-11-01 05:08:09 +01:00
return false;
2022-05-06 00:02:28 +02:00
}
path.Add(goal);
while(currentNode != start)
{
2022-11-02 18:07:22 +01:00
if(currentNode.previousNode != null)
{
path.Add(currentNode.previousNode);
currentNode = currentNode.previousNode;
}
2022-05-06 00:02:28 +02:00
}
path.Reverse();
2022-11-01 05:08:09 +01:00
logger?.Log(LogLevel.INFO, "Path found");
float distance = 0;
2022-11-02 18:07:22 +01:00
Node? prev = null;
2022-11-02 18:09:30 +01:00
TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).timeSpent);
2022-11-01 05:08:09 +01:00
foreach (Node n in path)
{
2022-11-02 18:07:22 +01:00
if(prev != null)
2022-11-01 05:08:09 +01:00
{
distance += Utils.DistanceBetweenNodes(prev, n);
}
prev = n;
2022-11-02 18:09:30 +01:00
logger?.Log(LogLevel.DEBUG, "lat {0:000.00000} lon {1:000.00000} traveled {5:0000.00}km in {2:G} / {3:G} Great-Circle to Goal {4:0000.00}", n.lat, n.lon, TimeSpan.FromSeconds(n.timeSpent), totalTime, n.goalDistance, distance);
2022-11-01 05:08:09 +01:00
}
return true;
2022-05-06 00:02:28 +02:00
}
/*
* Compares two nodes and returns the node closer to the goal
2022-11-01 05:08:09 +01:00
* -1 => n1 smaller n2
* 0 => n1 equal n2
* 1 => n1 larger n2
*/
2022-11-01 05:08:09 +01:00
private static int CompareDistance(Node n1, Node n2)
2022-05-06 00:02:28 +02:00
{
if (n1 == null || n2 == null)
return 0;
else
{
if (n1.goalDistance < n2.goalDistance)
return -1;
else if (n1.goalDistance > n2.goalDistance)
return 1;
2022-05-06 00:02:28 +02:00
else return 0;
}
2022-05-05 02:00:55 +02:00
}
2022-11-01 05:08:09 +01:00
/*
* Compares two nodes and returns the node with the shorter path
* -1 => n1 smaller n2
* 0 => n1 equal n2
* 1 => n1 larger n2
*/
private static int ComparePathLength(Node n1, Node n2)
{
if (n1 == null || n2 == null)
return 0;
else
{
2022-11-02 18:09:30 +01:00
if (n1.timeSpent < n2.timeSpent)
2022-11-01 05:08:09 +01:00
return -1;
2022-11-02 18:09:30 +01:00
else if (n1.timeSpent > n2.timeSpent)
2022-11-01 05:08:09 +01:00
return 1;
else return 0;
}
}
2022-05-05 02:00:55 +02:00
}
}