//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
});
}
}
