public void Enqueue(Node node) { if (array.Length == length) Enlarge(); array[length] = node; length++; for (int i = length - 2 ; i >= 0; i--) { if (array[i].TotalCost > node.TotalCost) { Node ghostNode = array[i + 1]; array[i + 1] = array[i]; array[i] = ghostNode; } else if (array[i].TotalCost <= node.TotalCost) break; } }
public void addNode(Node node) { _nodes.Add(node); }
public bool Contains(Node node) { return array.Any(node1 => node1 == node); }
private double heuristicCostEstimated(Node current, Node goal) { var currentX = current._p.X; var currentY = current._p.Y; var goalX = goal._p.X; var goalY = goal._p.Y; var differenceX = currentX - goalX; var differenceY = currentY - goalY; if (differenceX < 0) differenceX *= -1; if (differenceY < 0) differenceY *= -1; var total = (Math.Sqrt(Math.Pow(differenceX, 2) + Math.Pow(differenceY, 2))); return total; }
private List<Node> ReconstructPath(Node current) { var output = new List<Node>(); if (current.Previous != null) output = ReconstructPath(current.Previous); output.Add(current); return output; }
private int DistBetween(Node current, Node nextNode) { Edge edge = current._edges.First(e => e._nextNode == nextNode); if (edge._cost > 0) return edge._cost; var currentX = current._p.X; var currentY = current._p.Y; var neighborX = nextNode._p.X; var neighborY = nextNode._p.Y; var differenceX = currentX - neighborX; var differenceY = currentY - neighborY; if (differenceX < 0) differenceX *= -1; if (differenceY < 0) differenceY *= -1; var cost = (int)Math.Sqrt(Math.Pow(differenceX, 2) + Math.Pow(differenceY, 2)); edge._cost = cost; return cost; }
public void CreateListAStar(Node start, Node target) { List<Node> closedSet = new List<Node>(); // Set of nodes that are already evaluated. AStarQueue openSet = new AStarQueue(); //Set of tentative nodes to be evaluated, initially containing the start node List<Node> cameFrom = new List<Node>(); //List of navigated nodes int maxDistanceAtcf, distanceATCFLeft; List<Node> path = new List<Node>(); openSet.Enqueue(start); start.DistanceFromStart = 0; start.DistanceToGoal = (int)heuristicCostEstimated(start, target); start.TotalCost = start.DistanceFromStart + start.DistanceToGoal; maxDistanceAtcf = start.DistanceToGoal; distanceATCFLeft = start.DistanceToGoal; int bestScore = 0; while (openSet.GetLength() != 0) { var current = openSet.Dequeue(); closedSet.Add(current); current.aStarVisited = true; bool _tentativeIsBetter; if (current == target) { AStarTargets = ReconstructPath(current); break; } foreach (var edge in current._edges) { Node nextNode = edge._nextNode; var tentativeGScore = current.DistanceFromStart + DistBetween(current, nextNode); if (nextNode.aStarVisited && tentativeGScore >= nextNode.DistanceFromStart) continue; if (!openSet.Contains(nextNode) || tentativeGScore < nextNode.DistanceFromStart) { nextNode.DistanceToGoal = (int) heuristicCostEstimated(nextNode, target); nextNode.Previous = current; nextNode.DistanceFromStart = (int) tentativeGScore; nextNode.TotalCost = edge._nextNode.DistanceFromStart + edge._nextNode.DistanceToGoal; if (!openSet.Contains(nextNode)) openSet.Enqueue(nextNode); } } } }
/*public int getProgressPercent() { return (int)Math.Round(getProgress() * 100); }*/ private void UpdateXml(Node n, int v) { try { var x = new XmlDocument(); x.Load(filepath); string query = String.Format(startnode+n); var node = x.SelectSingleNode(query); node.InnerText = v.ToString(); x.Save(filepath); } catch (Exception) { UI.Notify("Could not save progess."); } }
public Edge(Node nextNode) { _nextNode = nextNode; _cost = 0; }
public Edge(Node nextNode, int cost) { _nextNode = nextNode; _cost = cost; }