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

Added pathlength to goaldistance criteria

Browse Source
This commit is contained in:
C9Glax 2022-05-06 00:31:26 +02:00
parent f6d5fe5905
commit 44511beb02
2 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Graph/Node.cs
View File

@ -9,13 +9,16 @@
public Node previousNode { get; set; } public Node previousNode { get; set; }
public double goalDistance { get; set; } public double goalDistance { get; set; }
public double pathLength { get; set; }
public Node(float lat, float lon) public Node(float lat, float lon)
{ {
this.lat = lat; this.lat = lat;
this.lon = lon; this.lon = lon;
this.edges = new List<Edge>(); this.edges = new List<Edge>();
this.previousNode = nullnode; this.previousNode = nullnode;
this.goalDistance = uint.MaxValue; this.goalDistance = double.MaxValue;
this.pathLength = double.MaxValue;
} }
public static Node nullnode = new Node(float.NaN, float.NaN); public static Node nullnode = new Node(float.NaN, float.NaN);
} }

37
astar/Astar.cs
View File

@ -7,6 +7,10 @@ namespace astar
public class Astar public class Astar
{ {
private Logger logger; private Logger logger;
/*
* Loads the graph, chooses two nodes at random and calls a*
*/
public Astar() public Astar()
{ {
this.logger = new Logger(LogType.Console, loglevel.DEBUG); this.logger = new Logger(LogType.Console, loglevel.DEBUG);
@ -18,14 +22,17 @@ 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}", n1.lat, n1.lon, n2.lat, n2.lon);
path = FindPath(n1, n2, ref this.logger); path = FindPath(ref nodes, n1, n2, ref this.logger);
} }
logger.Log(loglevel.INFO, "Path found"); logger.Log(loglevel.INFO, "Path found");
foreach (Node n in path) foreach (Node n in path)
logger.Log(loglevel.INFO, "{0} {1} Distance left {2}", n.lat, n.lon, n.goalDistance); logger.Log(loglevel.INFO, "lat {0:000.00000} lon {1:000.00000} traveled {2:0000.00} / {3:0000.00} beeline {4:0000.00}", n.lat, n.lon, n.pathLength, path.ElementAt(path.Count-1).pathLength, n.goalDistance);
} }
/*
* Resets the calculated previous nodes and distance to goal
*/
private static void Reset(ref Dictionary<ulong, Node> nodes) private static void Reset(ref Dictionary<ulong, Node> nodes)
{ {
foreach(Node n in nodes.Values) foreach(Node n in nodes.Values)
@ -35,26 +42,35 @@ namespace astar
} }
} }
private static List<Node> FindPath(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);
List<Node> toVisit = new List<Node>(); List<Node> toVisit = new List<Node>();
toVisit.Add(start); toVisit.Add(start);
Node currentNode = start; Node currentNode = start;
start.pathLength = 0;
start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
while(currentNode != goal && toVisit.Count > 0) while(currentNode != goal && toVisit.Count > 0)
{ {
logger.Log(loglevel.VERBOSE, "toVisit-length: {0} distance: {1}", toVisit.Count, currentNode.goalDistance); 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) foreach (Edge e in currentNode.edges)
{ {
if(e.neighbor.previousNode == Node.nullnode) if (e.neighbor.goalDistance == double.MaxValue)
{
toVisit.Add(e.neighbor);
e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal); 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; e.neighbor.previousNode = currentNode;
toVisit.Add(e.neighbor);
} }
} }
toVisit.Remove(currentNode); toVisit.Remove(currentNode); //"Mark" as visited
toVisit.Sort(CompareDistanceToGoal); toVisit.Sort(CompareDistanceToGoal);
currentNode = toVisit.First();
} }
List<Node> path = new List<Node>(); List<Node> path = new List<Node>();
@ -75,6 +91,9 @@ namespace astar
return path; return path;
} }
/*
* Compares two nodes and returns the node closer to the goal
*/
private static int CompareDistanceToGoal(Node n1, Node n2) private static int CompareDistanceToGoal(Node n1, Node n2)
{ {
if (n1 == null || n2 == null) if (n1 == null || n2 == null)