//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); } } } }
public void UpdateUnvisitedNeighbors(Node node, Node finishNode, List <List <Node> > grid) { SimplePriorityQueue <Node> unvisitedNeighbors = GetUnvisitedNeighbors(node, grid); foreach (Node neighbor in unvisitedNeighbors) { neighbor.GFunction = node.GFunction + 1 + neighbor.Weight; neighbor.PreviousNode = node; neighbor.HFunction = CalculateHFunction(neighbor, finishNode); int fFunction = neighbor.GFunction + neighbor.HFunction; if (OpenSet.Contains(neighbor)) { if (fFunction < OpenSet.GetPriority(neighbor)) { OpenSet.UpdatePriority(neighbor, fFunction); } } else { OpenSet.Enqueue(neighbor, fFunction); } } }
void ExpandNeighbour(IDirectedPath path, NeighbourHex neighbour) { if (!OpenSet.Contains(neighbour.Hex.Coords)) { var cost = StepCost(neighbour.Hex, neighbour.HexsideExit); if (cost > 0) { var newPath = path.AddStep(neighbour, cost); var key = Estimate(Heuristic, VectorGoal, Source.Coords, neighbour.Hex.Coords, newPath.TotalCost); TraceFindPathEnqueue(neighbour.Hex.Coords, key >> 16, (int)(key & 0xFFFFu)); Queue.Enqueue(key, newPath); } } }
private List <Node> GetSuccessors(Node node, bool DiagonalEnabled = false) { List <Node> successors = new List <Node>(); for (int row = node.RowIndex - 1; row <= node.RowIndex + 1; row++) { for (int col = node.ColumnIndex - 1; col <= node.ColumnIndex + 1; col++) { try { Node successor = map[row, col]; if (openSet.Contains(successor) || closedSet.Contains(successor)) { continue; } if (!successor.IsWalkable) { continue; } if (successor == node) { continue; } if (!DiagonalEnabled && IsDiagonalNeighbor(node, successor)) { continue; } successors.Add(successor); } catch { } } } return(successors); }
private void CalculateNeighbors(Node current, IReadOnlyList <Node> neighbors) { foreach (Node neighbor in neighbors) { if (ClosedSet.Contains(neighbor) || neighbor.IsWall) { continue; } int tempGScore = current.GScore + 1; bool newPath = false; if (OpenSet.Contains(neighbor)) { if (tempGScore < neighbor.GScore) { neighbor.GScore = tempGScore; newPath = true; } } else { neighbor.GScore = tempGScore; OpenSet.Add(neighbor); neighbor.State = NodeState.Open; newPath = true; } if (newPath) { neighbor.Hscore = Heuristic(neighbor, _finishNode); neighbor.FScore = neighbor.GScore + neighbor.Hscore; neighbor.Previous = 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); } } } }
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 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)); } } }
public override PathFinderResult FindPath() { GraphNodeHeuristic tempX, tempY; bool updateDist; int i, conn, distTemp; var numberOfSteps = 0; Distances[Graph.StartNode.NodeCode] = 0; // distance in firts node is zero OpenSet.Add(new GraphNodeHeuristic(Graph.StartNode, 0, 0)); //O(log n) while (OpenSet.Count > 0) //O(1) { numberOfSteps++; tempX = OpenSet.ExtractMin(); // get first suitable key and remove it (O(log n)) if (tempX.GraphNode.Equals(Graph.EndNode)) { return new PathFinderResult { Path = Path, PathDirections = PathDirections, Distances = Distances } } ; ClosedSet[tempX.NodeCode] = true; for (i = 0; i < 4; i++) // four directions { conn = tempX.NodeConnections[i]; if (conn == int.MaxValue || ClosedSet[conn]) { continue; //skip value, no connections } distTemp = Distances[tempX.NodeCode] + Graph.GraphBody[conn].NodeValue; tempY = new GraphNodeHeuristic(Graph.GraphBody[conn], distTemp, HeuristicComparer.ComputeHeuristic(Graph.GraphBody[conn])); if (!OpenSet.Contains(tempY)) //O(log 1) { OpenSet.Add(tempY); //O(log n) updateDist = true; } else if (distTemp < Distances[tempY.NodeCode]) { updateDist = true; } else { updateDist = false; } if (updateDist) { OpenSet.UpdateDistance(tempY); // O(log n) Path[tempY.NodeCode] = tempX.GraphNode; PathDirections[tempY.NodeCode] = GraphDirections.GetDirectionCharCode((byte)i); Distances[tempY.NodeCode] = distTemp; } } } // no path found return(new PathFinderResult { Path = null, PathDirections = null, Distances = null }); } }