2022-05-05 16:12:40 +02:00
|
|
|
|
using Logging;
|
|
|
|
|
using Graph;
|
2022-05-05 02:00:55 +02:00
|
|
|
|
|
|
|
|
|
namespace astar
|
|
|
|
|
{
|
|
|
|
|
public class Astar
|
|
|
|
|
{
|
2022-05-06 00:31:26 +02:00
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* 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-05-06 00:31:26 +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));
|
2022-05-06 00:31:26 +02:00
|
|
|
|
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;
|
2022-05-06 00:31:26 +02:00
|
|
|
|
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.");
|
|
|
|
|
}
|
2022-05-06 00:31:26 +02:00
|
|
|
|
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);
|
2022-05-06 00:31:26 +02:00
|
|
|
|
//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-05-06 00:31:26 +02:00
|
|
|
|
{
|
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;
|
2022-05-06 00:31:26 +02:00
|
|
|
|
toVisit.Add(e.neighbor);
|
2022-05-06 00:02:28 +02:00
|
|
|
|
}
|
|
|
|
|
}
|
2022-11-01 05:08:09 +01:00
|
|
|
|
|
2022-05-06 00:31:26 +02: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-05-11 22:17:44 +02:00
|
|
|
|
{
|
2022-11-01 05:08:09 +01:00
|
|
|
|
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
|
2022-05-11 22:17:44 +02:00
|
|
|
|
}
|
|
|
|
|
else
|
2022-05-06 00:02:28 +02:00
|
|
|
|
{
|
2022-05-11 19:45:35 +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
|
|
|
|
}
|
|
|
|
|
|
2022-05-06 00:31:26 +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-05-06 00:31:26 +02:00
|
|
|
|
*/
|
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;
|
2022-05-11 22:17:44 +02:00
|
|
|
|
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
|
|
|
|
}
|
|
|
|
|
}
|