public static List <Node> AStarBuTheBook(Node start, Node goal) { ; if (start.IsObstacle || goal.IsObstacle) { Console.WriteLine("Can't run A*! Enter proper from and to, one of them is an obsttacle!"); return(null); } // The set of nodes already evaluated var closedSet = new List <Node>(); // The set of currently discovered nodes that are not evaluated yet. // Initially, only the start node is known. var openSet = new List <Node> { start }; // For each node, which node it can most efficiently be reached from. // If a node can be reached from many nodes, cameFrom will eventually contain the // most efficient previous step. var cameFrom = new List <Node>(); while (openSet.Count != 0) { openSet = openSet.OrderBy(arg => arg.DistanceToGoal).ToList(); var current = openSet[0]; if (current.Position == goal.Position) { return(reconstructPath(cameFrom, current)); } var neighbours = NodesArray.Neighbours(current); openSet.Remove(current); closedSet.Add(current); foreach (var neighbour in neighbours) { if (closedSet.Exists(arg => arg.Position == neighbour.Position)) { continue; // Ignore the neighbor which is already evaluated. } if (!openSet.Exists(arg => arg.Position == neighbour.Position)) { openSet.Add(neighbour); // Discover a new node } var tentativeDistance = current.DistanceToStart + Node.MetricsAStar(current, neighbour); if (tentativeDistance > neighbour.DistanceToStart) { continue; // This is not a better path. } // This path is the best until now. Record it! if (!cameFrom.Exists(arg => arg.Position == current.Position)) { cameFrom.Add(current); } neighbour.DistanceToStart = tentativeDistance; neighbour.DistanceToGoal = neighbour.DistanceToStart + Node.MetricsAStar(neighbour, goal); } } return(null); }
public static List <Node> AStar(Node from, Node to) { var finalPath = new List <Node>(); if (from.IsObstacle || to.IsObstacle) { Console.WriteLine("Can't run A*! Enter proper from and to, one of them is an obsttacle!"); return(finalPath); } var current = new Node(from) { DistanceToGoal = Node.MetricsAStar(@from, to), Visited = NodeState.Processed }; var observed = new List <Node> { current }; while (current.Position != to.Position) { var query = NodesArray.Neighbours(current); query = query.Where(node => !node.IsObstacle).ToList(); foreach (var node in query) { if (!observed.Exists(arg => arg.Position == node.Position)) { node.DistanceToGoal = Node.MetricsAStar(node, to); node.Visited = NodeState.Discovered; observed.Add(node); } } observed = observed.OrderBy(arg => arg.Visited).ThenBy(arg => arg.DistanceToGoal).ToList(); if (observed[0].Visited != NodeState.Processed) { current = observed[0]; observed[0].Visited = NodeState.Processed; } else { Console.WriteLine("No path was found"); return(finalPath); } } observed = observed.Where(informer => informer.Visited == NodeState.Processed).ToList(); finalPath.Add(observed[0]); for (var i = 0; i < observed.Count; ++i) { var maxIndex = i; for (var j = i + 1; j < observed.Count; ++j) { if (StraightLine.OnOneLine(observed[i], observed[j], NodesArray)) { maxIndex = j; } } if (maxIndex != i) { var points = StraightLine.FindMiddlePoints(observed[i], observed[maxIndex]); points.RemoveAt(0); foreach (var point in points) { var coordinatesX = Convert.ToInt32(point.X); var coordinatesY = Convert.ToInt32(point.Y); finalPath.Add(NodesArray.Array[coordinatesX, coordinatesY]); } i = maxIndex - 1; } } finalPath.Reverse(); return(finalPath); }