AStar/astar/Astar.cs

191 lines
6.7 KiB
C#
Raw Normal View History

using Logging;
using Graph;
2022-11-13 14:02:27 +01:00
using Graph.Utils;
2022-05-05 02:00:55 +02:00
namespace astar
{
public class Astar
{
2022-11-13 14:14:55 +01:00
private Dictionary<Node, float> timeRequired = new();
private Dictionary<Node, float> goalDistance = new();
private Dictionary<Node, Node> previousNode = new();
2022-11-13 14:02:27 +01:00
public Route FindPath(Graph.Graph graph, Node start, Node goal, Logger? logger)
2022-05-06 00:02:28 +02:00
{
2022-11-03 19:35:04 +01:00
logger?.Log(LogLevel.INFO, "From {0:000.00000}#{1:000.00000} to {2:000.00000}#{3:000.00000} Great-Circle {4:00000.00}km", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal)/1000);
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-13 14:14:55 +01:00
SetTimeRequiredToReach(start, 0);
SetDistanceToGoal(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)));
while (toVisit.Count > 0 && GetTimeRequiredToReach(toVisit[0]) < GetTimeRequiredToReach(goal))
2022-05-06 00:02:28 +02:00
{
currentNode = toVisit.First();
2022-11-13 14:02:27 +01:00
logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, timeRequired[currentNode], goalDistance[currentNode]);
//Check all neighbors of current node
2022-05-06 00:02:28 +02:00
foreach (Edge e in currentNode.edges)
{
2022-11-13 14:14:55 +01:00
if (GetTimeRequiredToReach(e.neighbor) > GetTimeRequiredToReach(currentNode) + e.time)
{
2022-11-13 14:14:55 +01:00
SetDistanceToGoal(e.neighbor, Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)));
SetTimeRequiredToReach(e.neighbor, GetTimeRequiredToReach(currentNode) + e.time);
SetPreviousNodeOf(e.neighbor, currentNode);
if (!toVisit.Contains(e.neighbor))
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-13 14:02:27 +01:00
2022-11-13 14:14:55 +01:00
if(GetPreviousNodeOf(goal) != null)
{
2022-11-01 05:08:09 +01:00
logger?.Log(LogLevel.INFO, "Way found, shortest option.");
2022-11-03 19:14:14 +01:00
currentNode = goal;
}
else
2022-05-06 00:02:28 +02:00
{
2022-11-03 19:14:14 +01:00
logger?.Log(LogLevel.INFO, "No path between {0:000.00000}#{1:000.00000} and {2:000.00000}#{3:000.00000}", start.lat, start.lon, goal.lat, goal.lon);
2022-11-13 14:02:27 +01:00
return new Route(new List<Step>(), false, float.MaxValue, float.MaxValue);
2022-05-06 00:02:28 +02:00
}
2022-11-17 20:36:08 +01:00
#pragma warning disable CS8604, CS8600 // Route was found, so has to have a previous node and edges
2022-11-03 19:14:14 +01:00
List<Node> tempNodes = new();
tempNodes.Add(goal);
2022-05-06 00:02:28 +02:00
while(currentNode != start)
{
2022-11-13 14:14:55 +01:00
tempNodes.Add(GetPreviousNodeOf(currentNode));
currentNode = GetPreviousNodeOf(currentNode);
2022-11-03 19:14:14 +01:00
}
tempNodes.Reverse();
2022-11-13 14:02:27 +01:00
List<Step> steps = new();
float totalDistance = 0;
2022-11-03 19:14:14 +01:00
for(int i = 0; i < tempNodes.Count - 1; i++)
{
Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]);
2022-11-13 14:14:55 +01:00
steps.Add(new Step(tempNodes[i], e, GetTimeRequiredToReach(tempNodes[i]), GetDistanceToGoal(tempNodes[i])));
2022-11-13 14:02:27 +01:00
totalDistance += e.distance;
2022-05-06 00:02:28 +02:00
}
2022-11-01 05:08:09 +01:00
2022-11-13 14:14:55 +01:00
Route _route = new Route(steps, true, totalDistance, GetTimeRequiredToReach(goal));
2022-11-13 14:02:27 +01:00
2022-11-01 05:08:09 +01:00
logger?.Log(LogLevel.INFO, "Path found");
2022-11-03 19:14:14 +01:00
if(logger?.level > LogLevel.INFO)
2022-11-01 05:08:09 +01:00
{
2022-11-03 19:14:14 +01:00
float time = 0;
float distance = 0;
2022-11-03 19:35:04 +01:00
logger?.Log(LogLevel.DEBUG, "Route Distance: {0:00000.00km} Time: {1}", _route.distance/1000, TimeSpan.FromSeconds(_route.time));
2022-11-03 19:14:14 +01:00
for(int i = 0; i < _route.steps.Count; i++)
2022-11-01 05:08:09 +01:00
{
2022-11-03 19:14:14 +01:00
Step s = _route.steps[i];
time += s.edge.time;
distance += s.edge.distance;
2022-11-13 14:02:27 +01:00
logger?.Log(LogLevel.DEBUG, "Step {0:000} From {1:000.00000}#{2:000.00000} To {3:000.00000}#{4:000.00000} along {5:0000000000} after {6} and {7:0000.00}km", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(timeRequired[s.start]), distance/1000);
2022-11-01 05:08:09 +01:00
}
}
2022-11-03 19:14:14 +01:00
return _route;
2022-11-17 20:36:08 +01:00
#pragma warning restore CS8604, CS8600
2022-05-06 00:02:28 +02:00
}
2022-11-17 20:36:08 +01: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-13 14:02:27 +01:00
private int CompareDistance(Node n1, Node n2)
2022-05-06 00:02:28 +02:00
{
if (n1 == null || n2 == null)
return 0;
else
{
2022-11-13 14:14:55 +01:00
if (GetDistanceToGoal(n1) < GetDistanceToGoal(n2))
2022-05-06 00:02:28 +02:00
return -1;
2022-11-13 14:14:55 +01:00
else if (GetDistanceToGoal(n1) > GetDistanceToGoal(n2))
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
*/
2022-11-13 14:02:27 +01:00
private int ComparePathLength(Node n1, Node n2)
2022-11-01 05:08:09 +01:00
{
if (n1 == null || n2 == null)
return 0;
else
{
2022-11-13 14:14:55 +01:00
if (GetTimeRequiredToReach(n1) < GetTimeRequiredToReach(n2))
2022-11-01 05:08:09 +01:00
return -1;
2022-11-13 14:14:55 +01:00
else if (GetTimeRequiredToReach(n1) > GetTimeRequiredToReach(n2))
2022-11-01 05:08:09 +01:00
return 1;
else return 0;
}
}
2022-11-03 19:14:14 +01:00
2022-11-13 14:14:55 +01:00
private float GetTimeRequiredToReach(Node n)
{
if (timeRequired.TryGetValue(n, out float t))
{
return t;
}
else
{
return float.MaxValue;
}
}
private void SetTimeRequiredToReach(Node n, float t)
{
if (!timeRequired.TryAdd(n, t))
timeRequired[n] = t;
}
private float GetDistanceToGoal(Node n)
{
if (goalDistance.TryGetValue(n, out float t))
{
return t;
}
else
{
return float.MaxValue;
}
}
private void SetDistanceToGoal(Node n, float d)
{
if (!goalDistance.TryAdd(n, d))
goalDistance[n] = d;
}
private Node? GetPreviousNodeOf(Node n)
{
if(previousNode.TryGetValue(n, out Node? t))
{
return t;
}else
{
return null;
}
}
private void SetPreviousNodeOf(Node n, Node p)
{
if (!previousNode.TryAdd(n, p))
previousNode[n] = p;
}
2022-05-05 02:00:55 +02:00
}
}