public List<Node> Astar(Node start, Node goal) { if (start == null || goal == null){ return null; } if (start == goal) { return new List<Node> {start}; } foreach(Node node in objectManager.NodeManager.nodes) { node.Reset(); } MinHeap openSet = new MinHeap(start); start.IsInOpenSet = true; start.gScore = 0; start.fScore = start.gScore + Heuristic_cost_estimate (goal, start); Node current = null; while (openSet.Count() > 0) { current = openSet.GetRoot (); current.IsInOpenSet = false; current.IsInClosedSet = true; if (current == goal) { return Reconstruct_path (start, goal); } foreach (Node neighbor in current.BorderTiles) { if(neighbor == null || !neighbor.IsWalkable || neighbor.IsInClosedSet) continue; // if the new gscore is lower replace it int tentativeGscore = current.gScore + Heuristic_cost_estimate (current, neighbor); if (!neighbor.IsInOpenSet || tentativeGscore < neighbor.gScore) { neighbor.parent = current; neighbor.gScore = tentativeGscore; neighbor.fScore = neighbor.gScore + Heuristic_cost_estimate (goal, neighbor); if (!neighbor.IsInOpenSet){ openSet.Add (neighbor); neighbor.IsInOpenSet = true; } else { openSet.Reevaluate(neighbor); } } } } // Fail return null; }
public void Astar() { if (start == null || goal == null) { done = true; return; } if (start == goal) { done = true; return; } MinHeap openSet = new MinHeap(start); start.isInOpenSet = true; start.gScore = 0; start.fScore = start.gScore + Heuristic_cost_estimate(goal, start); int numSteps = 0; Node current = null; while (openSet.Count() > 0) { current = openSet.GetRoot(); current.isCurrent = true; current.isInOpenSet = false; current.isInClosedSet = true; ControlLogic(current, numSteps); numSteps++; current.isCurrent = false; if (current == goal) { pathToDestination = Reconstruct_path(start, goal); done = true; return; } foreach (Node neighbor in current.getNeighbors()) { if (neighbor == null || !neighbor.isWalkable || neighbor.isInClosedSet) { continue; } // if the new gscore is lower replace it int tentativeGscore = current.gScore + Heuristic_cost_estimate(current, neighbor); if (!neighbor.isInOpenSet || tentativeGscore < neighbor.gScore) { neighbor.parent = current; neighbor.gScore = tentativeGscore; neighbor.fScore = neighbor.gScore + Heuristic_cost_estimate(goal, neighbor); if (!neighbor.isInOpenSet) { openSet.Add(neighbor); neighbor.isInOpenSet = true; } else { openSet.Reevaluate(neighbor); } } } } // Fail done = true; return; }
private void Astar() { start.isInOpenSetOfThread[id] = true; MinHeap openSet = new MinHeap(start, id); openSet.Add(start); start.gScores[id] = 0; start.fScores[id] = start.gScores[id] + Heuristic_cost_estimate(goal, start); int numSteps = 0; while (!finished) { Node current = openSet.GetRoot(); current.isInOpenSetOfThread[id] = false; current.isInClosedSet = true; current.isCurrent = true; ControlLogic(current, numSteps); numSteps++; current.isCurrent = false; if (current.checkedByThread == 0) { if (current.fScores[id] < (int)L && current.gScores[id] + brotherThread.F - brotherThread.Heuristic_cost_estimate(start, current) < (int)L) { foreach (Node neighbor in current.getNeighbors()) { if (neighbor != null && neighbor.isWalkable) { int cost = Heuristic_cost_estimate(current, neighbor); if (neighbor.checkedByThread == 0 && neighbor.gScores[id] > current.gScores[id] + cost) { neighbor.gScores[id] = current.gScores[id] + cost; neighbor.fScores[id] = neighbor.gScores[id] + Heuristic_cost_estimate(goal, neighbor); neighbor.parents[id] = current; if (!neighbor.isInOpenSetOfThread[id]) { neighbor.isInOpenSetOfThread[id] = true; openSet.Add(neighbor); } else { openSet.Reevaluate(neighbor); } if (neighbor.gScores[brotherThread.id] != int.MaxValue && neighbor.gScores[brotherThread.id] + neighbor.gScores[id] < (int)L) { lock (L) { if (neighbor.gScores[brotherThread.id] + neighbor.gScores[id] < (int)L) { L = neighbor.gScores[brotherThread.id] + neighbor.gScores[id]; endNode = current; brotherThread.endNode = neighbor; } } } } } } } current.checkedByThread = id; } if (openSet.Count() > 0) { F = openSet.Peek().fScores[id]; } else { finished = true; } } }