public void ManageNeighbours(AStarNode state) { foreach (var item in state.Neighbours) { if (!item.Closed) { if (!item.Opened) //element, spoza listy zamkniętych i otwartych { item.Parent = state; // zapisz rodzica item.Opened = true; item.ComputeF(InitialState.Position, Goal.Position); //oblicz współczynnik F AddToOpen(item); //dodaj do otwartych } else//element już znajduje się na liście otwartych { AStarNode tmp = OpenList.Find(p => p.Name == item.Name); //znajdź na liście ten element if (tmp != null && item.F < tmp.F) // odśwież współczynnik F { item.Parent = tmp.Parent = state; item.ComputeF(InitialState.Position, Goal.Position); tmp.F = item.F; } } } } }
public IList <AStarNode> Expand(AStarNode state) { AddInitial(state); // dodaj startowy element do listy zamkniętych(odwiedzonych) AStarNode tmp; while (!IsGoal(ClosedList.Last())) { OpenList.OrderBy(p => p.F); //lista otwartych posortowana wg najmniejszej wagi współczynnika F tmp = OpenList.First(); //weź pierwszy element z otwartej listy(najlepiej prosperujący) RemoveFirst(OpenList); //przenieś do zamkniętej tmp.Opened = false; tmp.Closed = true; AddToClosed(tmp); ManageNeighbours(ClosedList.Last()); //dodaj sąsiadów do otwartej listy lub zaktualizuj wsp F i rodzica } AStarNode parent = Goal; //uzupełnij listę ze ścieżką poprzez wybranie elementu docelowego i rekursywnie dodaj wszystkie elementy połączone while (parent != null) { StackTrace.Insert(0, parent); parent = parent.Parent; } return(StackTrace); }
private void BTN_m3u_Click(object sender, EventArgs e) { LBOX_mp3.Items.Clear(); TXT_m3u.Text = string.Empty; OpenList.FileName = string.Empty; size = 0; enable_check = false; if (OpenList.ShowDialog() == DialogResult.OK) { manager = new M3uManager(OpenList.FileName); StringDictionary list = manager.List; if (list != null) { TXT_m3u.Text = OpenList.FileName; foreach (string title in list.Keys) { LBOX_mp3.Items.Add(title, true); size = size + manager.Get_File_size(title); } enable_check = true; Lbl_Size2.Text = size.ToString(); LbL_TotalFiles.Text = list.Count.ToString(); } else { MessageBox.Show("There is a problem with this list"); OpenList.FileName = string.Empty; } } }
public SearchResult <T> Search() { T state = Problem.InitialState; Node <T> node = new Node <T>(state); if (Problem.GoalTest(state)) { return(new SearchResult <T>(node)); } Frontier.Put(node); OpenList.Add(state); while (!Frontier.IsEmpty) { node = Frontier.Take(); state = node.State; ClosedList.Add(state); foreach (IAction <T> action in Problem.Actions(state)) { Node <T> childNode = node.ChildNode(Problem, action); T childState = childNode.State; if (!ClosedList.Contains(childState) && !OpenList.Contains(childState)) { if (Problem.GoalTest(childState)) { return(new SearchResult <T>(childNode)); } Frontier.Put(childNode); OpenList.Add(childState); } } } return(new SearchResult <T>(null)); }
public void OpenSquare(int y, int x, int[] parent, int moveCost, double heuristic, bool newSquare) { if (!newSquare) { foreach (OpenSquare op in OpenList) { if (op.Y == y && op.X == x) { newSquare = true; break; } } } if (!newSquare) { OpenList.Add(new OpenSquare(y, x)); MapStatus.RemoveAll((c) => c.X == x && c.Y == y); MapStatus.Add(new CellInfo(heuristic, null, true, false, x, y)); } CellInfo cell = GetCellInfo(y, x); cell.Parent = parent; cell.MovementCost = moveCost; }
public async Task <string> CreateDatabase() { string databaseId = string.Empty; Dictionary <string, string> properties = new Dictionary <string, string>(); OpenList = cacheManager.Get <string>(DatabaseType.Open.GetEnumDescription()).Result; UsedList = cacheManager.Get <string>(DatabaseType.Used.GetEnumDescription()).Result; BeingCreatedList = cacheManager.Get <string>(DatabaseType.BeingCreated.GetEnumDescription()).Result; if (TotalAvailableDatabase > 0) { CommonFunctions.LogMetrics($"Returning from cache"); databaseId = OpenList.FirstOrDefault(); Remove(databaseId, DatabaseType.Open); } else if (TotalAvailableDatabase + TotalUsedDatabase >= ServiceConfigSettings.MaximumDatabases) { CommonFunctions.LogMetrics($"Max count of databases reached"); throw new Exception("Please wait as the system has reached maximum number of databases"); } else { CommonFunctions.LogMetrics($"Creating physical database"); string dbID = "the-goods-mobile-" + Guid.NewGuid().ToString(); Add(databaseId, DatabaseType.BeingCreated); databaseId = dbOperations.CreateDatabase(dbID).Result; Remove(databaseId, DatabaseType.BeingCreated); } Add(databaseId, DatabaseType.Used); return(databaseId); }
public void CloseSquare(int y, int x) { OpenList.RemoveAll((os) => os.X == x && os.Y == y); CellInfo cell = GetCellInfo(y, x); cell.Opened = false; cell.Closed = true; }
// Constructor public AStar(World w, aNode start, aNode goal) { openList = new OpenList<aNode>(); closedList = new ArrayList(); startNode = start; goalNode = goal; thisWorld = w; }
public MainUI() { InitializeComponent(); OpenList.DisplayMember = "Text"; OpenList.ClearSelected(); OpenList2.DisplayMember = "Text"; OpenList2.ClearSelected(); }
private void ArbIn1_CheckedChanged(object sender, EventArgs e) { OpenList.Enabled = !arbIn1.Checked; OpenList.ClearSelected(); if (!arbIn1.Checked && !arbIn2.Checked && (OpenList.SelectedIndex != -1 || OpenList2.SelectedIndex != -1)) { Start.Enabled = false; } }
public void Reset() { for (int i = 0; i < NodeCost.Length; i++) { NodeCost[i] = 0; ParentNode[i] = -1; } OpenList.Clear(); ClosedList.Clear(); }
private State <T>[] EmptyQueueIntoArray() { int size = OpenListSize; State <T>[] array = new State <T> [size]; for (int i = 0; i < size; ++i) { array[i] = OpenList.Dequeue(); } return(array); }
public List <Node> AStar() { // Pop off lowest F int? min = OpenList.Min(n => n.F); Node current = OpenList.Where(n => n.F == min).First(); OpenList.Remove(current); // Check for goal if (current.Equals(Goal)) { ClosedList.Push(current); return(ClosedList.Reverse().ToList()); } else { //possibleMoves = GetPassibleNodes(current); OpenList = GetPassibleNodes(current); List <Node> toRemove = new List <Node>(); OpenList.ForEach(n => { if (ClosedList.Contains(n)) { toRemove.Add(n); } else { GetF(n); } }); toRemove.ForEach(n => OpenList.Remove(n)); //possibleMoves.ForEach(n => //{ // if (!ClosedList.Contains(n)) // Dont move to an already visited Node // { // GetF(n); // if (!OpenList.Contains(n)) // { // n.Parent = current; // OpenList.Add(n); // } // } //}); } ClosedList.Push(current); return(null); }
public bool FindTheShortesPath(Node Start, Node Target) { Node CurrentNode = Start; OpenList.Add(CurrentNode); for (; OpenList.Count > 0;) { CurrentNode = OpenList[0]; if (CurrentNode.Equals(Target)) { VisitedNodes.Add(Target); for (int Index = 0; Index < VisitedNodes.Count - 1; Index++) { ShortesPath.Add(InnerGraph.FindEdge(VisitedNodes[Index], VisitedNodes[Index + 1])); } return(true); } OpenList.Remove(CurrentNode); ClosedList.Add(CurrentNode); foreach (Node Inheritor in CurrentNode.Inheritors.Where(Node => Node != null && Node.Index != Node.Inheritors.Length - 1)) { if (!ClosedList.Contains(Inheritor)) { if (!OpenList.Contains(Inheritor)) { Inheritor[Inheritor.Index] = CurrentNode; Inheritor.HeuristicPathWeight = CalculateHeuristic(Inheritor, Target); Inheritor.GainedPathWeight = InnerGraph.FindEdge(CurrentNode, Inheritor).Weight; Inheritor.TotalPathWeight = Inheritor.GainedPathWeight + Inheritor.HeuristicPathWeight; OpenList.Add(Inheritor); OpenList = OpenList.OrderBy(Node => Node.TotalPathWeight).ToList <Node>(); } } } VisitedNodes.Add(CurrentNode); } return(true); }
//Initializes Search public Search(Node[,] nodes, Node start, Node end, SearchAlgorithm searchAlgorithm) { //Set starting variables Start = start; End = end; SearchAlgorithm = searchAlgorithm; //Starting node should always be open, so add it to the OpenNodes list. OpenList.Add(start); //Set start node as in OpenNodes. start.InOpenNodes = true; //Apply Start F cost start.F = CalculateAStarH(start, end); }
private static void check_tile( Game_Unit unit, Vector2 loc, int parent, int mov, Vector2 target_loc, OpenList <Vector2> open_list, ClosedListRoute <Vector2> closed_list, bool dijkstras = false, bool use_euclidean_distance = false) { // return if terrain type of this tile doesn't have stats //Yeti int move_cost = pathfinding_terrain_cost(unit, loc); bool pass; int h = 0; if (Doors.Contains(loc) && move_cost < 0) { // I don't quite understand why I left the move cost negative for doors //Debug move_cost = -10;// (unit.mov + 1) * 10; //Debug pass = true; h = (unit.mov + 1) * 10; } else { pass = passable(unit, loc, loc != target_loc) || (loc == target_loc && Global.game_map.is_off_map(target_loc, false)); } if (pass) { int g = move_cost + closed_list.get_g(parent); if (mov < 0 || g <= mov * 10) { h += (dijkstras ? 0 : distance(loc, target_loc, use_euclidean_distance)); // If fog and AI controlled and the unit can't see this tile //if (Global.game_map.fow && !unit.is_ally && !Global.game_map.fow_visibility[unit.team].Contains(loc)) //Debug if (Global.game_map.fow && Global.game_state.ai_active && !Global.game_map.fow_visibility[unit.team].Contains(loc)) { // Make the tile less desirable for the unit to cross h += unit.mov * 10; } int f = g + h; int on_list = open_list.search(loc); if (on_list > -1) { open_list.repoint(on_list, parent, f, g); } else { open_list.add_item(loc, parent, f, g, pass); } } } }
bool PushOpenList(int StandardPos, int OpenPos) { if (!ClosedList.Contains(OpenPos)) { if (!OpenList.Contains(OpenPos)) { OpenList.Add(OpenPos); ((Tile)Tiles[OpenPos]).Goal = CalcuGoal(StandardPos, OpenPos); ((Tile)Tiles[OpenPos]).Heuristic = CalcuHeuristic(OpenPos, EndPos); ((Tile)Tiles[OpenPos]).Fitness = ((Tile)Tiles[OpenPos]).Goal + ((Tile)Tiles[OpenPos]).Heuristic; ((Tile)Tiles[OpenPos]).ParentTile = StandardPos; return(true); } } return(false); }
private bool ProcessCell(int originX, int originY, int direction, double moveCost) { var delta = GetDelta(direction); int x = originX + delta.X; int y = originY + delta.Y; double fNew; double gNew; double hNew; if (!IsValid(x, y)) { return(false); } if (x == DestinationX && y == DestinationY) { CellDetails[x, y].ParentX = originX; CellDetails[x, y].ParentY = originY; SetNodes(); return(true); } if (ClosedList[x, y] == false && Map.Tiles[x, y].Walkable) { gNew = CellDetails[originX, originY].GValue + moveCost; hNew = CalculateHValue(x, y); fNew = gNew + hNew; if (CellDetails[x, y].FValue > fNew) { OpenList.Add(new NodeScore(fNew, new Point(x, y))); var curCell = CellDetails[x, y]; curCell.FValue = fNew; curCell.GValue = gNew; curCell.HValue = hNew; curCell.ParentX = originX; curCell.ParentY = originY; } } return(false); }
public void init(ref INode[,] map) { this.map = map; foreach (INode node in map) { if (node.property == nodeProp.START) { start = node; } if (node.property == nodeProp.GOAL) { goal = node; } } curTile = null; isCalculating = false; if (OpenList == null) { OpenList = new List <INode>(); } else { OpenList.Clear(); } if (CloseList == null) { CloseList = new List <INode>(); } else { CloseList.Clear(); } if (PathList == null) { PathList = new List <INode>(); } else { PathList.Clear(); } }
public List <Node> GetPath() { List <Node> path = AStar(); while (path == null) { path = AStar(); if (OpenList.Count() == 0) { return(null); } } ; return(path); }
private void ListUser_DropDownOpened(object sender, EventArgs e) { try { elementResum.Clear(); ListUser.Items.Clear(); OpenList?.Invoke(this, EventArgs.Empty); for (int i = 0; i < elementResum.Count; i++) { ListUser.Items.Add("1." + (elementResum[i])); } } catch (Exception ex) { MessageBox.Show(ex.Message); } }
public SearchResult <T> Search() { T state = Problem.InitialState; HeuristicNode <T> node = new HeuristicNode <T>(state, 1); if (Problem.GoalTest(state)) { return(new SearchResult <T>(node)); } PriorityQueue.Push(node); OpenList.Add(state); while (!PriorityQueue.IsEmpty) { node = PriorityQueue.Pop(); state = node.State; ClosedList.Add(state); foreach (IAction <T> action in Problem.Actions(state)) { HeuristicNode <T> childNode = node.ChildNode(Problem, action, Heuristic); T childState = childNode.State; if (ClosedList.Contains(childState) || OpenList.Contains(childState)) { if (PriorityQueue.Contains(childNode) && childNode.HeuristicCost > node.HeuristicCost) { PriorityQueue.Push(childNode); OpenList.Add(childState); } } if (!ClosedList.Contains(childState) && !OpenList.Contains(childState)) { if (Problem.GoalTest(childState)) { return(new SearchResult <T>(childNode)); } PriorityQueue.Push(childNode); OpenList.Add(childState); } } } return(new SearchResult <T>(null)); }
/// <summary> /// Searcher's abstract method overriding /// </summary> /// <param name="searchable"> to search on </param> /// <returns>the solution that BFS found</returns> public override Solution <T> Search(ISearchable <T> searchable) { lock (locker2) { AddToOpenList(searchable.GetInitialState()); } HashSet <State <T> > closed = new HashSet <State <T> >(); State <T> goal = searchable.GetGoalState(); while (OpenListSize > 0) { State <T> state = PopOpenList(); // removes the best state closed.Add(state); // add it to the closed hash if (state.Equals(goal)) { return(BackTrace(goal)); // back traces through the parents } lock (locker) { // returns a list of states with state as a parent List <State <T> > succerssors = searchable.GetAllPossibleStates(state); foreach (State <T> s in succerssors) { if (!closed.Contains(s) && !OpenList.Contains(s)) { s.CameFrom = state; AddToOpenList(s); } // the cost of the new way is better else if (OpenList.Contains(s)) { s.CameFrom = state; UpdateStateIfPathBetter(s); } } } } return(null); }
/// <summary> /// 重置 /// </summary> public void ReSet() { if (StartCell != null) { MapBmpFillRectangle(StartCell, this._whiteSmoke); StartCell = null; } if (GoalCell != null) { MapBmpFillRectangle(GoalCell, this._whiteSmoke); GoalCell = null; } MapBmpFillRectangles(StoneCells, this._whiteSmoke); StoneCells.Clear(); MapBmpFillRectangles(ClosedList, this._whiteSmoke); ClosedList.Clear(); MapBmpFillRectangles(OpenList, this._whiteSmoke); OpenList.Clear(); }
/// <summary> /// Searches the specified searchable. /// </summary> /// <param name="searchable">The searchable.</param> /// <returns></returns> public override Solution <T> Search(ISearchable <T> searchable)// Searcher's abstract method overriding { State <T> initialState = searchable.GetInitialState(); OpenList.Enqueue(initialState); HashSet <State <T> > closed = new HashSet <State <T> >(); while (OpenListSize > 0) { State <T> currentState = PopOpenList(); // inherited from Searcher, removes the best state closed.Add(currentState); if (currentState.Equals(searchable.GetGoalState())) { return(Backtrace(searchable.GetGoalState(), GetNumberOfNodesEvaluated())); // private method, back traces through the parents } // calling the delegated method, returns a list of state List <State <T> > succerssors = searchable.GetAllPossibleStates(currentState); foreach (State <T> currentSuccessor in succerssors) { if (!closed.Contains(currentSuccessor) && !OpenList.Contains(currentSuccessor)) { currentSuccessor.CameFrom = currentState; currentSuccessor.Cost = currentState.Cost + searchable.GetStatesCost(currentState, currentSuccessor); OpenList.Enqueue(currentSuccessor); } else { if (!closed.Contains(currentSuccessor)) { AdjustPriorityForState(currentSuccessor, currentState, currentState.Cost + searchable.GetStatesCost(currentState, currentSuccessor)); } } } } return(null); }
public void Initialize(Node start, Node goal) { Start = start; Sprite = start; Goal = goal; // Initialize All Nodes on board with coordinates and passible for (int i = 0; i < Size; i++) { State.Add(new List <Node>()); for (int j = 0; j < Size; j++) { State[i].Add(new Node(i, j, true)); } } // Set 10% of Nodes as Impassible SetImpassibleNodes(); // Add Start Node to Open List GetF(Start); OpenList.Add(Start); }
private bool IsInOpen(Point cellPt) { return(OpenList.FindIndex(record => record.Location.Equals(cellPt)) != -1); }
/// <summary> /// Generic search algorithm /// </summary> private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int? depthLimit = null) { var openList = new OpenList(comparator); var closedList = new ClosedList(); var cache = new HeuristicCache(goalState, heuristic); var nodesGenerated = 0; var nodesPrevGenerated = 0; // add initial node to open list openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate)); while (true) { // if nothing on open list, fail if (openList.Count == 0) { return new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic); } // get next node to expand var node = openList.Pop(); closedList.Push(node); // if at goal state, success if (node.State.Equals(goalState)) { return new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic); } // if at depth limit, don't generate successors if (depthLimit != null && node.Depth == depthLimit) { continue; } var daughters = node.Successors(cost, cache.Evaluate); foreach (var daughter in daughters) { nodesGenerated++; // if this state is already in open list, replace old node with new node if g-hat is better var foundInOpen = openList.Find(daughter.State); if (foundInOpen != null) { nodesPrevGenerated++; if (daughter.Ghat < foundInOpen.Ghat) { openList.Replace(foundInOpen, daughter); } } else { // else if this state is already in closed list, move from closed to open if g-hat is better var foundInClosed = closedList.Find(daughter.State); if (foundInClosed != null) { nodesPrevGenerated++; if (daughter.Ghat < foundInClosed.Ghat) { openList.Push(daughter); closedList.Remove(foundInClosed); } } else { // else didn't find in open or closed, add to open openList.Push(daughter); } } } } }
public static Vector2 find_open_tile(Vector2 target_loc, int id, Vector2 loc) { reset_move_costs(); // Prepare outside variables for pathfinding Game_Unit unit = Global.game_map.units[id]; if (map_data_needs_updated(unit)) { update_map_data(unit); last_id = id; } if (Global.game_map.width == 0) { return(Config.OFF_MAP); } //Prepare pathfinding variables OpenList <Vector2> open_list = new OpenList <Vector2>(); ClosedListRoute <Vector2> closed_list = new ClosedListRoute <Vector2>(); int temp_parent = -1; Vector2 temp_loc = Vector2.Zero; int temp_f = 0; int temp_g = 0; int temp_h = 0; bool temp_accessible = true; Vector2[] check_loc = new Vector2[] { new Vector2(0, -1), new Vector2(0, 1), new Vector2(-1, 0), new Vector2(1, 0) }; Vector2 test_loc; bool route_found = false; // Start pathfinding temp_g = 0; temp_h = manhatten_dist(loc, target_loc); temp_f = temp_g + temp_h; open_list.add_item(loc, temp_parent, temp_f, temp_g, temp_accessible); for (; ;) { if (open_list.size <= 0) { break; } OpenItem <Vector2> lowest_f_item = open_list.get_lowest_f_item(); temp_loc = lowest_f_item.Loc; temp_parent = lowest_f_item.Parent; temp_f = lowest_f_item.Fcost; temp_g = lowest_f_item.Gcost; temp_accessible = lowest_f_item.Accessible; temp_parent = closed_list.add_item(temp_loc, temp_parent, temp_f, temp_g, temp_accessible); open_list.remove_open_item(); if (Global.game_map.get_unit(temp_loc) == null) { route_found = true; break; } else { bool reverse = (rand.Next(2) == 0); reverse = false; for (int i = 0; i < check_loc.Length; i++) { test_loc = temp_loc + check_loc[reverse ? 3 - i : i]; if (Global.game_map.is_off_map(test_loc)) { continue; } if (closed_list.already_added(test_loc)) { continue; } check_tile(unit, test_loc, temp_parent, -1, target_loc, open_list, closed_list, true); } } } unit_distance = 0; if (route_found) { return(temp_loc); } return(Config.OFF_MAP); }
public static List <Vector2> get_route(Vector2 target_loc, int mov, int id, Vector2 loc, bool through_doors = false, bool ignore_doors = false) { reset_move_costs(); // Prepare outside variables for pathfinding Game_Unit unit = Global.game_map.units[id]; if (map_data_needs_updated(unit)) { update_map_data(unit, through_doors, ignore_doors); last_id = id; } //Restrict_To_Map = !Global.game_map.is_off_map(loc, false) && !Global.game_map.is_off_map(target_loc, false); //Debug Restrict_To_Map = !Global.game_map.is_off_map(loc) && !Global.game_map.is_off_map(target_loc); //Prepare pathfinding variables OpenList <Vector2> open_list = new OpenList <Vector2>(); ClosedListRoute <Vector2> closed_list = new ClosedListRoute <Vector2>(); int temp_parent = -1; Vector2 temp_loc = Vector2.Zero; int temp_f = 0; int temp_g = 0; int temp_h = 0; bool temp_accessible = true; Vector2[] check_loc = new Vector2[] { new Vector2(0, -1), new Vector2(0, 1), new Vector2(-1, 0), new Vector2(1, 0) }; Vector2 test_loc; bool route_found = false; bool use_euclidean_distance = mov == -1; // Start pathfinding temp_g = 0; temp_h = distance(loc, target_loc, use_euclidean_distance); temp_f = temp_g + temp_h; open_list.add_item(loc, temp_parent, temp_f, temp_g, temp_accessible); for (; ;) { if (open_list.size <= 0) { break; } OpenItem <Vector2> lowest_f_item = open_list.get_lowest_f_item(); temp_loc = lowest_f_item.Loc; temp_parent = lowest_f_item.Parent; temp_f = lowest_f_item.Fcost; temp_g = lowest_f_item.Gcost; temp_accessible = lowest_f_item.Accessible; temp_parent = closed_list.add_item(temp_loc, temp_parent, temp_f, temp_g, temp_accessible); open_list.remove_open_item(); if (temp_loc == target_loc) { route_found = true; break; } else { for (int i = 0; i < check_loc.Length; i++) { test_loc = temp_loc + check_loc[i]; #if DEBUG if (Global.game_map.is_off_map(test_loc, Restrict_To_Map)) { int test = 0; } #endif // If the checked location isn't the target but is off the map, and off the map is not allowed if (test_loc != target_loc && Global.game_map.is_off_map(test_loc, Restrict_To_Map)) { continue; } // If the location is already on the closed list if (closed_list.already_added(test_loc)) { continue; } check_tile(unit, test_loc, temp_parent, mov, target_loc, open_list, closed_list, use_euclidean_distance: use_euclidean_distance); } } } unit_distance = 0; if (route_found) { unit_distance = closed_list.get_g(temp_parent) / 10; var route = closed_list .get_route(temp_parent) .ToArray(); return(Enumerable.Range(0, route.Length - 1) .Select(x => route[x] - route[x + 1]) .ToList()); } return(null); }
public static Path <SS, DS> AStarLookAhead(DS Start, SS ss, int ComputationLimit, StateVisualizer <SS, DS> sv, ref uint Expansions, ref uint Generations, bool Static, Operator <SS, DS>[] Actions, Dictionary <string, Metric> HAdjusted, Heuristic <SS, DS> Heuristic, Goal <SS, DS> Goal, Metric Weight, bool RTAStar) { if (ComputationLimit <= 0) { throw new ArgumentException("Computation Limit Invalid"); } DS[] Neighbors = Start.Expand(Actions, ss, Static).ToArray( ); int IterationExpansions = 1; Expansions++; Dictionary <DS, Metric> GNeighs = new Dictionary <DS, Metric>( ); Dictionary <DS, Metric> HNeighs = new Dictionary <DS, Metric>( ); Dictionary <DS, Operator <SS, DS> > OpNeighs = new Dictionary <DS, Operator <SS, DS> >( ); LinkedList <DS> LookAheadNeighbors = new LinkedList <DS>( ); foreach (DS Neighbor in Neighbors) { GNeighs.Add(Neighbor, Neighbor.Path( ).Cost); OpNeighs.Add(Neighbor, Neighbor.Path( ).Actions.First.Value); Neighbor.ResetPath( ); Metric HNeigh = null; if (HAdjusted != null && HAdjusted.TryGetValue(Neighbor.ToString( ), out HNeigh)) { HNeighs.Add(Neighbor, HNeigh); } else { LookAheadNeighbors.AddFirst(Neighbor); } } Dictionary <DS, PriorityQueue <Metric, DS> > OpenLists = new Dictionary <DS, PriorityQueue <Metric, DS> >( ); #if OpenGl HashSet <DS> OpenStates = new HashSet <DS>( ); #endif HashSet <DS> ClosedList = new HashSet <DS>( ); ClosedList.Add(Start); foreach (DS Neighbor in LookAheadNeighbors) { var ol = new PriorityQueue <Metric, DS>( ); ol.Enqueue(h(Neighbor, HAdjusted, Heuristic, Goal, Actions, ss), Neighbor); OpenLists.Add(Neighbor, ol); #if OpenGl OpenStates.Add(Neighbor); #endif } while (true) { LinkedList <DS> OpenNeighbors = new LinkedList <DS>(LookAheadNeighbors); foreach (DS Neighbor in LookAheadNeighbors) { PriorityQueue <Metric, DS> OpenList; OpenLists.TryGetValue(Neighbor, out OpenList); #if OpenGl if (sv != null) { sv.VisualizeAlg(ss, ss.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( ) { List = OpenStates as HashSet <GenericGridWorldDynamicState>, HAdjusted = HAdjusted, }); } #endif if (OpenList.IsEmpty( )) { HNeighs.Add(Neighbor, new Metric(double.PositiveInfinity)); OpenNeighbors.Remove(Neighbor); continue; } DS Cur = OpenList.Dequeue( ); #if OpenGl OpenStates.Remove(Cur); #endif ClosedList.Add(Cur); if (Goal.IsSatisfiedBy(ss, Cur)) { HNeighs.Add(Neighbor, Cur.Path( ).Cost); OpenNeighbors.Remove(Neighbor); continue; } else { if (++IterationExpansions > ComputationLimit) { HNeighs.Add(Neighbor, Cur.Path( ).Cost + Weight * h(Cur, HAdjusted, Heuristic, Goal, Actions, ss)); OpenNeighbors.Remove(Neighbor); break; } Expansions++; foreach (DS e in Cur.Expand(Actions, ss, Static)) { if (!ClosedList.Contains(e) && !OpenList.Contains(e)) { OpenList.Enqueue(Weight * h(e, HAdjusted, Heuristic, Goal, Actions, ss) + gComputer.G(ss, e, Goal), e); #if OpenGl OpenStates.Add(e); #endif } Generations++; } } } LookAheadNeighbors = OpenNeighbors; if (IterationExpansions > ComputationLimit || LookAheadNeighbors.Count == 0) { break; } } foreach (DS Neighbor in LookAheadNeighbors) { PriorityQueue <Metric, DS> OpenList; OpenLists.TryGetValue(Neighbor, out OpenList); if (OpenList.IsEmpty( )) { HNeighs.Add(Neighbor, new Metric(double.PositiveInfinity)); } else { DS Cur = OpenList.Dequeue( ); HNeighs.Add(Neighbor, Cur.Path( ).Cost + Weight * h(Cur, HAdjusted, Heuristic, Goal, Actions, ss)); } } Metric Best = null; Operator <SS, DS> BestOp = null; Metric SecondBest = null; foreach (DS Neighbor in Neighbors) { Metric HNeigh = null; Metric GNeigh = null; Operator <SS, DS> OpNeigh = null; HNeighs.TryGetValue(Neighbor, out HNeigh); GNeighs.TryGetValue(Neighbor, out GNeigh); OpNeighs.TryGetValue(Neighbor, out OpNeigh); if (HNeigh != null) { Metric FNeigh = GNeigh + HNeigh; if (Best == null || Best > FNeigh) { BestOp = OpNeigh; Best = FNeigh; } else if (SecondBest == null || SecondBest > FNeigh) { SecondBest = FNeigh; } } } if (RTAStar && SecondBest != null) { if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < SecondBest) { h(Start, SecondBest, HAdjusted); } } else if ((RTAStar && SecondBest == null) || Best == null) { h(Start, new Metric(double.PositiveInfinity), HAdjusted); } else if (Best != null) { if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < Best) { h(Start, Best, HAdjusted); } } if (BestOp == null) { return(null); } else { return(Operator <SS, DS> .MakePath(BestOp)); } }
public void doPathFinding() { Node firstNode = new Node(m_current, null, m_end, m_start); finalPath = new EnemyPathing(); OpenList openedList = new OpenList(); openedList.insertToOpenList(firstNode); ClosedList closedList = new ClosedList(); SearchLvl search_lvl = new SearchLvl(); while (openedList.isEmpty() == false) { Node secondNodes = openedList.get0(); Node secondNode = new Node(secondNodes.getPosition(), secondNodes.returnPrev(), m_end, m_start); openedList.remove0Fromlist(); closedList.insertToClosedList(secondNode); if (Physics2D.OverlapBox(new Vector2(secondNode.getPosition().x, secondNode.getPosition().y), new Vector2(m_halfWidth / 2, m_halfWidth / 2), 0, targetMask)) { results = secondNode; break; } List <Node.Position> adjacentPositions = search_lvl.getAdjacentNodes(secondNode.getPosition().x, secondNode.getPosition().y, tileWid); for (int i = 0; i < adjacentPositions.Count; i++) { if (closedList.isInClosedList(adjacentPositions[i])) { continue; } int inter = openedList.findFromOpenList(secondNode.getPosition()); //returns -1 if there was no match, iterator cant be negative anyway Node previousNode = new Node(adjacentPositions[i], secondNode, m_end, m_start); if (inter != -1) { //has been found in open list if (openedList.returnAt(inter).getDistanceFromStart() > previousNode.getDistanceFromStart()) //the new distance is smaller { openedList.returnAt(inter).setPrev(secondNode); //setting the previous node that is found at the index } } else { //lineDrawer(previousNode.getPosition().x, previousNode.getPosition().y); //This method just creates a box, used for debugging purposes only! openedList.insertToOpenList(previousNode); } } } if (results != null) { while (results != null) { if (results.returnPrev() == null) { finalPath.insertToPath(results); results = null; donePathFind = true; } else { //Debug.DrawLine(new Vector2(results.getPosition().x, results.getPosition().y), new Vector2(results.returnPrev().getPosition().x, results.returnPrev().getPosition().y), Color.red, 1, false); finalPath.insertToPath(results); //draw a line to prev results = results.returnPrev(); } } finalPath.Reversed(); } else { donePathFind = true; finalPath = null; //NOT FOUND } }