public void SolveMaze(bool visualizeSolver) { while (OpenSet.Count != 0) { int winnerIndex = CalculateWinnerIndex(); Node current = OpenSet[winnerIndex]; OpenSet.Remove(current); ClosedSet.Add(current); current.State = NodeState.Closed; var neighbors = current.Neighbors; CalculateNeighbors(current, neighbors); // Calculates the current path CalculatePath(current); // We reached the end if this evaluates to true if (current == _finishNode) { MazeSolved = true; ReportProgress(); return; } ReportProgress(); if (visualizeSolver) { Thread.Sleep(10); } } }
//This function is the main difference between A* and Theta* algorithms private void UpdateNode(Node Current, Node Neighbour) { //If there is LoS, ignore current and use the path from its parent to the neighbour node if (GridManager.Instance.LineOfSight(Current.Parent, Neighbour)) { //Make sure this pathway is cheaper before updating it if (Current.Parent.GScore + GridManager.FindHeuristic(Current.Parent, Neighbour) < Neighbour.GScore) { Neighbour.GScore = Current.Parent.GScore + GridManager.FindHeuristic(Current.Parent, Neighbour); Neighbour.Parent = Current; Neighbour.ToggleParentIndicator(true); Neighbour.PointIndicator(Neighbour.GetDirection(Current)); if (OpenSet.Contains(Neighbour)) { OpenSet.Remove(Neighbour); } OpenSet.Add(Neighbour); } } else { if (Current.GScore + GridManager.FindHeuristic(Current, Neighbour) < Neighbour.GScore) { Neighbour.GScore = Current.GScore + GridManager.FindHeuristic(Current, Neighbour); Neighbour.Parent = Current; if (OpenSet.Contains(Neighbour)) { OpenSet.Remove(Neighbour); } } } }
private void PerformStep() { if (openSet.Count == 0) { SetStateToFinished(); return; } currentNode = openSet.Pop(); if (IsPathFound(currentNode)) { HandleFoundPath(currentNode); SetStateToFinished(); return; } List <Node> successors = GetSuccessors(currentNode, DiagonalsEnabled); foreach (Node successor in successors) { successor.MovementCost = currentNode.MovementCost + MovementCost(successor, currentNode); successor.Parent = currentNode; openSet.Add(successor); } openSet.Remove(currentNode); closedSet.Add(currentNode); }
public bool IsFinished() { if (Queue.TryDequeue(out var item)) { var path = item.Value; var coords = path.PathStep.Coords; OpenSet.Remove(coords); if (!ClosedSet.Contains(coords)) { PathfinderExtensions.TraceFindPathDequeue(GetType().Name, coords, path, item.Key, 0); if (item.Key < BestSoFar) { Partner.Heuristic(coords).IfHasValueDo(heuristic => { if (path.TotalCost + Partner.FrontierMinimum() - heuristic < BestSoFar) { Hexside.ForEach(hexside => ExpandHex(path, hexside)); } }); } ClosedSet.Add(coords); } return(!Queue.Any()); } return(true); }
private void ExpandHex(IDirectedPath path, Hexside hexside) { var here = path.PathStep.Coords; var there = here.GetNeighbour(hexside); if (!ClosedSet.Contains(there)) { TryStepCost(here, hexside).IfHasValueDo(cost => { if (path.TotalCost + cost < BestSoFar || !OpenSet.ContainsKey(there)) { Heuristic(there).IfHasValueDo(heuristic => { var key = path.TotalCost + cost + heuristic; var newPath = path.AddStep(there, HexsideDirection(hexside), cost); PathfinderExtensions.TraceFindPathEnqueue(there, key, 0); if (!OpenSet.TryGetValue(there, out var oldPath)) { OpenSet.Add(there, newPath); Queue.Enqueue(key, newPath); } else if (newPath.TotalCost < oldPath.TotalCost) { OpenSet.Remove(there); OpenSet.Add(there, newPath); Queue.Enqueue(key, newPath); } SetBestSoFar(newPath, PartnerPath(there)); }); } }); } }
internal bool IsFinished() { HexKeyValuePair <int, IDirectedPath> item; if (Queue.TryDequeue(out item)) { var path = item.Value; var coords = path.PathStep.Hex.Coords; OpenSet.Remove(coords); if (!ClosedSet.Contains(coords)) { TraceFindPathDequeue(GetType().Name, coords, path.TotalCost, path.HexsideExit, item.Key, 0); if (item.Key < BestSoFar && path.TotalCost + Partner.FrontierMinimum() - Partner.Heuristic(coords) < BestSoFar ) { HexsideExtensions.HexsideList.ForEach(hexside => ExpandHex(path, hexside)); } ClosedSet.Add(coords); } return(!Queue.Any()); } return(true); }
private void ExpandHex(IDirectedPath path, Hexside hexside) { var here = path.PathStep.Hex; var there = Board[here.Coords.GetNeighbour(hexside)]; if (there != null && !ClosedSet.Contains(there.Coords)) { var cost = StepCost(here, hexside, there); if ((cost > 0) && (path.TotalCost + cost < BestSoFar || !OpenSet.ContainsKey(there.Coords)) ) { var key = path.TotalCost + cost + Heuristic(there.Coords); var newPath = path.AddStep(there, HexsideDirection(hexside), cost); TraceFindPathEnqueue(there.Coords, key, 0); IDirectedPath oldPath; if (!OpenSet.TryGetValue(there.Coords, out oldPath)) { OpenSet.Add(there.Coords, newPath); Queue.Enqueue(key, newPath); } else if (newPath.TotalCost < oldPath.TotalCost) { OpenSet.Remove(there.Coords); OpenSet.Add(there.Coords, newPath); Queue.Enqueue(key, newPath); } SetBestSoFar(newPath, GetPartnerPath(there.Coords)); } } }
public override void IterateOpenSet() { OpenSetIterations++; //Path doesnt exist if the openset runs out if (OpenSet.Count <= 0) { Log.Print("no path found"); GridManager.Instance.HideAllParentIndicators(); FindingPathway = false; return; } //Take the front node from the open set Node Current = OpenSet[0]; OpenSet.Remove(Current); Current.Closed = true; //Pathway is completed once the end node has been reached if (Current == PathEnd) { Log.Print("Pathway found after " + OpenSetIterations + " iterations."); FindingPathway = false; GridManager.Instance.HideAllParentIndicators(); foreach (Node Step in GridManager.Instance.GetCompletedPathway(PathStart, PathEnd)) { Step.SetType(NodeType.Pathway); } return; } //Process all the nodes neighbours foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current)) { //Skip neighbours already searched if (Neighbour.Closed || Neighbour.Opened) { continue; } //Move this neighbour onto the open list, open it and update its parent OpenSet.Add(Neighbour); Neighbour.Opened = true; Neighbour.Parent = Current; Neighbour.ToggleParentIndicator(true); Neighbour.PointIndicator(Neighbour.GetDirection(Current)); } }
public override void IterateOpenSet() { //Track how many iterations have taken place to find this pathway so far OpenSetIterations++; //If the open set is empty, then no pathway was able to be found if (OpenSet.Count <= 0) { //Print a failure message and reset the grid Log.Print("Unable to find a valid pathway using A* algorithm."); FindingPathway = false; GridManager.Instance.HideAllParentIndicators(); return; } //Get the cheapest node currently being stored in the open set Node Current = GridManager.Instance.FindCheapestNode(OpenSet); OpenSet.Remove(Current); //When Current matches the end node, the pathway is ready to be reconstructed if (Current == PathEnd) { //Announce the pathway has been found and how long it took to find Log.Print("A* pathfinding completed after " + OpenSetIterations + " iterations."); //Hide all parent indicators GridManager.Instance.HideAllParentIndicators(); //Grab and display the final pathway List <Node> Pathway = GridManager.Instance.GetCompletedPathway(PathStart, PathEnd); foreach (Node PathStep in Pathway) { PathStep.SetType(NodeType.Pathway); } //Finalize the process FindingPathway = false; return; } //Remove the current node from the open set, then iterate over its neighbours OpenSet.Remove(Current); foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current)) { if (Current.GScore < Neighbour.GScore) { //Update this as the preferred way to travel Neighbour.Parent = Current; Neighbour.ToggleParentIndicator(true); Neighbour.PointIndicator(Neighbour.GetDirection(Current)); //Update neighbours score values Neighbour.GScore = Current.GScore; Neighbour.FScore = Neighbour.GScore + GridManager.FindHeuristic(Neighbour, PathEnd); //Add neighbour to open set if its not there already if (!OpenSet.Contains(Neighbour)) { OpenSet.Add(Neighbour); } } } }
/// <summary> /// 路径规划 /// </summary> /// <param name="snId">起始点编号</param> /// <param name="gnId">目标点编号</param> /// <returns>规划所得路径的节点列表</returns> public StoreHeap PathPanning(int snId, int gnId) { if (snId != gnId) { Node startNode = NodeList.FindNode(snId); //起始点 Node goalNode = NodeList.FindNode(gnId); //目标点 // 设置起始点和目标点的累积代价为0 startNode.CurrentCost = goalNode.CurrentCost = 0; foreach (Node node1 in NodeList) { node1.NGoal = goalNode; } // 添加起始点到开放集合 OpenSet.Add(startNode); DataTable dt = Graph.DS.Tables["NodeInfo"]; while (true) { // 当前点为开放集合中启发代价最小的点 Node currentNode = OpenSet.GetMinCostNode(); // 如果当前点为目标点,则循环结束 if (currentNode.Index == goalNode.Index) { // 设置目标点的父节点为关闭集合中的最后一个点 goalNode.NParent = ClosedSet.Pop(); break; } // 将当前点从开放集合中删去并添加到关闭集合 OpenSet.Remove(currentNode); ClosedSet.Add(currentNode); for (int i = 0; i < Graph.Nodes; i++) { if (Graph.Matrix[currentNode.Index - 1, i] != 0) { Node node = NodeList.FindNode((int)dt.Rows[i]["id"]); node.CurrentCost = currentNode.CurrentCost + Graph.Matrix[currentNode.Index - 1, i]; // 如果节点存在于关闭集合,则跳过后续步骤,直接下一个循环 if (ClosedSet.IsExist(node)) { continue; } // 如果节点不在开放集合,则将节点加入到开放集合 else if (!OpenSet.IsExist(node)) { node.NParent = currentNode; OpenSet.Add(node); } else { // 如果当前点到查询到的点的代价比现在的大,则更新节点的父节点 if (OpenSet.FindNode(node.Index).CurrentCost >= node.CurrentCost) { node.NParent = currentNode; OpenSet.ReplaceNode(node); } } } } } StoreHeap finalPath = new StoreHeap(); //最终路径的节点列表 Node recallnode = goalNode; finalPath.Add(recallnode); // 只要当前点还存在父节点,则将其都加入到最终路径的节点列表 while (recallnode.HasParent()) { recallnode = recallnode.NParent; finalPath.Add(recallnode); } finalPath.Reverse(); return(finalPath); } // 如果起始点和目标点为同一点,则返回一个包含起始点的节点列表 else { StoreHeap result = new StoreHeap(); result.Add(NodeList.FindNode(snId)); return(result); } }
public override void IterateOpenSet() { //Track how many times the open set has been iterated over OpenSetIterations++; //If the open set is empty, then no pathway could be found if (OpenSet.Count <= 0) { //Print a failure message and reset the grid Log.Print("Unable to find a valid pathway using Theta* algorithm, after a total " + OpenSetIterations + " iterations."); FindingPathway = false; GridManager.Instance.HideAllParentIndicators(); return; } //Grab the node from open set with the lowest FScore value, move it from the OpenSet to the ClosedSet Node Current = GridManager.Instance.FindCheapestNode(OpenSet); OpenSet.Remove(Current); ClosedSet.Add(Current); //If the Current node is the EndNode, then the pathway has been found if (Current == PathEnd) { //Announce the pathway has been found Log.Print("Theta* pathfinding complete after " + OpenSetIterations + " iterations."); //Hide all nodes parent indicators GridManager.Instance.HideAllParentIndicators(); //Get the completed pathway List <Node> FinalPathway = GridManager.Instance.GetCompletedPathway(PathStart, PathEnd); //Change all pathway node types so the completed pathway is displayed foreach (Node PathStep in FinalPathway) { PathStep.SetType(NodeType.Pathway); } //Finalize the pathfinding process FindingPathway = false; return; } //Go through all the current nodes neighbours foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current)) { //Ignore neighbours in the closed list if (ClosedSet.Contains(Neighbour)) { continue; } if (!OpenSet.Contains(Neighbour)) { Neighbour.GScore = Mathf.Infinity; Neighbour.Parent = null; } UpdateNode(Current, Neighbour); } }
private bool CalculateNextState() { if (OpenSet.Count > 0) { Square current = OpenSet[0]; // choose a square with a lowest F-cost // H-cost - distance from a current node to end node + // G-cost - distance from starting node to current node // if F-cost is the same, then we choose the one with the lowest H-cost for (int i = 1; i < OpenSet.Count; i++) { if (OpenSet[i].FCost < current.FCost || OpenSet[i].FCost == current.FCost && OpenSet[i].HCost < current.HCost) { current = OpenSet[i]; } } // remove current from open set // add it to closed set, saying that it's // no longer going to be operated on OpenSet.Remove(current); ClosedSet.Add(current); // if we reached the end, escape if (current == End) { RetracePath(Start, End); return(false); } //RetracePath(Start, current); // get all the neighbors of the current node foreach (Square neighbor in GetNeighbors(current)) { // skipping neighbors that are already in closed set // and are obstacles if (ClosedSet.Contains(neighbor) || neighbor.Obstacle) { continue; } // G-cost of a neighbor int costToNeighbor = current.GCost + Heuristics(current, neighbor); // if open set doesn't contain neighbor // then the value if neighbor.GCost is going to be 0 // we will need to update neighbor's cost if we meet it second time if (costToNeighbor < neighbor.GCost || !OpenSet.Contains(neighbor)) { neighbor.GCost = costToNeighbor; neighbor.HCost = Heuristics(neighbor, End); // remember previous node of neighbor, so that // we can retrace the path neighbor.Previous = current; if (!OpenSet.Contains(neighbor)) { OpenSet.Add(neighbor); } } } return(true); } return(false); }
public override SchedulareState Execute(Schedulare schedulare, ShiftsContainer shiftsContainer) { InitParams(schedulare, shiftsContainer); var schedulareState = GetSchedulareState(schedulare.DeepClone(), shiftsContainer, TreeRoot); OpenSet.Add(schedulareState); ExecuteStopwatch.Start(); UpdateThreshold(schedulareState); while (!OpenSet.IsNullOrEmpty()) { var currState = GetCurrentState(OpenSet); UpdateCurrentBestSolution(currState); OpenSet.Remove(currState); CloseSet.Add(currState); var currNode = currState.Node; PrintDebugData(shiftsContainer, currState); if (IsGoal()) { UpdateCurrentBestSolution(currState); break; } // if the current node is full schedulare but it is not goal yet // remove the node from open list and look for another solutions if (IsSchedulareFull(currNode.Value, shiftsContainer)) { UpdateCurrentBestSolution(currState); OpenSet.Remove(currState); CloseSet.Add(currState); continue; } // create and add child nodes #region build new nodes foreach (var emp in shiftsContainer.EmployeeConstraints.Select(x => x.Name)) { var newNodeSchedulare = currNode.Value.DeepClone(); var currShiftToAssin = GetIncompleteShift(newNodeSchedulare, shiftsContainer); // modify schdulare currShiftToAssin.Workers.Add(new Worker() { Name = emp }); // add new node to the tree - currNode var childNode = currNode.AddChild(newNodeSchedulare); // get new state var newNodeState = GetSchedulareState(newNodeSchedulare, shiftsContainer, childNode); // add new state to openSet OpenSet.Add(newNodeState); } #endregion } PrintDebugData(shiftsContainer, CurrentBestSolution); var ret = CurrentBestSolution; CurrentBestSolution = null; IsFinished = false; ExecuteStopwatch.Reset(); return(ret); }
public override void IterateOpenSet() { //Track how many iterations have taken place to find this pathway OpenSetIterations++; //Once the openset has been exhausted its time to complete the pathway if (OpenSet.Count <= 0) { //Get the completed pathway List <Node> Pathway = GridManager.Instance.GetCompletedPathway(PathStart, PathEnd); //If the path is empty, then no pathway was able to be found between the target nodes if (Pathway == null) { //Print a failure message and reset the grid Log.Print("Unable to find a valid pathway using Dijkstras algorithm."); FindingPathway = false; GridManager.Instance.HideAllParentIndicators(); return; } //Announce the pathway has been found Log.Print("Dijkstras pathfinding completed after " + OpenSetIterations + " iterations."); //Hide all the neighbour indicators GridManager.Instance.HideAllParentIndicators(); //Change the type of all nodes in the pathway to display it in the game foreach (Node PathStep in Pathway) { PathStep.SetType(NodeType.Pathway); } //Complete the process FindingPathway = false; return; } //Find the new current node, then iterate through all its neighbours Node Current = GridManager.Instance.FindCheapestNode(OpenSet); OpenSet.Remove(Current); foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current)) { //Ignore nodes not listed in the open set if (!OpenSet.Contains(Neighbour)) { continue; } //check if its cheaper to travel over this neighbour float NeighbourCost = Current.FScore + GridManager.FindHeuristic(Current, Neighbour); if (NeighbourCost < Neighbour.FScore) { //update this neighbour as the best way to travel Neighbour.FScore = NeighbourCost; Neighbour.Parent = Current; Neighbour.ToggleParentIndicator(true); Neighbour.PointIndicator(Neighbour.GetDirection(Current)); } } }