/**
* Runs a generic AStar search
* The search can look through action nodes or pathfinding nodes and find a path from the start to the Goal
* @param the ai module,needed to generate neighbours
*/
public void RunAStar(Agent ai)
{
float g, h, f; //The g,h and f values needed for the formula f = g + h
int numberOfNeighbours = 0;
short neighbour;
AStarNode neighbourNode;
CurrentNode = Map.CreateANode(End);
//Add the first node to the open list
Storage.AddToOpenList(CurrentNode, Map);
h = Goal.GetHeuristicDistance(ai, CurrentNode, true);
CurrentNode.G = 0.0f;
CurrentNode.H = h;
CurrentNode.F = h;
//if (ai.debugGOAP) Debug.Log(Time.timeSinceLevelLoad + " RunAStar " + CurrentNode.NodeID);
/**
* We come to the main body of A*, we continue until a solution is found
*/
while (true)
{
//Get the node with the lowest f from the front of the Storage
CurrentNode = Storage.RemoveCheapestOpenNode(Map);
//Add the node to the closed list
if (CurrentNode != null)
{
Storage.AddToClosedList(CurrentNode, Map);
}
else
{
break;
}
//Check whether we've reached our Goal
//The abstract Goal handles the different Goal conditions for nav and planning
if (Goal.IsAStarFinished(CurrentNode))
{
break; //We've finished, break out
}
/**
* Next job is to get the neighbours of the current node
* Iterate over them calculate their f,g,h and add to the open list if required
*/
numberOfNeighbours = Map.GetNumAStarNeighbours(CurrentNode);
for (short i = 0; i < numberOfNeighbours; i++)
{
neighbour = Map.GetAStarNeighbour(CurrentNode, i);
if (neighbour == -1) //If neighbour is invalid quit
{
break;
}
AStarNode.E_AStarFlags flags = Map.GetAStarFlags(neighbour);
//check whether the node is flagged as open
if (flags == AStarNode.E_AStarFlags.Open)
{
neighbourNode = Storage.FindInOpenList(neighbour);
}
else if (flags == AStarNode.E_AStarFlags.Closed)
{
/**
* Get node from the closed list
* If the new f for this node is better than previously then remove the node from the closed list
* If it is greater than previously then skip this neighbour
*/
neighbourNode = Storage.FindInClosedList(neighbour);
}
else if (Goal.IsAStarNodePassable(neighbour))
{
neighbourNode = Map.CreateANode(neighbour);
}
else
{
continue;
}
//If out best path so far is through the neighbour
//then theres no need to re-assess
if (neighbourNode == null || CurrentNode.Parent == neighbourNode)
{
continue;
}
/**
* Get the new g,h and f for this neighbour node
*/
g = CurrentNode.G + (Goal.GetActualCost(CurrentNode, neighbourNode));
h = Goal.GetHeuristicDistance(ai, neighbourNode, false);
f = g + h;
/**
* Now need to check if the new f is more expensive to get to this
* neighbor node from the current node than from its previous parent.
*/
if (f >= neighbourNode.F)
{
continue;
}
neighbourNode.F = f;
neighbourNode.G = g;
neighbourNode.H = h;
if (flags == AStarNode.E_AStarFlags.Closed)
{
Storage.RemoveFromClosedList(neighbourNode.NodeID, Map);
}
// Finally add the neighbour to the open list
Storage.AddToOpenList(neighbourNode, Map);
neighbourNode.Parent = CurrentNode;
}
}
}
/**
* Adds the specified node to the open list.
* The list is sorted so the node must be inserted in order
* @param the node to add
*/
public void AddToOpenList(AStarNode node, AStarMap map)
{
AStarNode currNode = headOfOpenList;
AStarNode PreviousNode = null;
AStarNode NextNode = null;
if (map.GetAStarFlags(node.NodeID) == AStarNode.E_AStarFlags.Open)
{
return;
}
if (currNode == null)
{
headOfOpenList = node;
openSize = 1;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
node.Flag = AStarNode.E_AStarFlags.Open;
}
else
{
bool inserted = false;
while (!inserted)
{
NextNode = currNode.Next;
PreviousNode = currNode.Previous;
//New node has a higher value than what is already in the open list
if (currNode.F > node.F)
{ //If no Previous node then we have the start node
if (PreviousNode == null)
{
currNode.Previous = node;
node.Next = currNode;
node.Previous = null;
headOfOpenList = node;
node.Flag = AStarNode.E_AStarFlags.Open;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
inserted = true;
openSize++;
}
else
{
PreviousNode.Next = node;
node.Previous = PreviousNode;
node.Next = currNode;
currNode.Previous = node;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
node.Flag = AStarNode.E_AStarFlags.Open;
inserted = true;
openSize++;
}
}
else if (currNode.F == node.F)
{ //Check if the current node has a higher precendence than existing
if (map.CompareNodes(currNode, node))
{ //Can insert node in before the current node
if (PreviousNode != null)
{
PreviousNode.Next = node;
currNode.Previous = node;
node.Previous = PreviousNode;
node.Next = currNode;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
node.Flag = AStarNode.E_AStarFlags.Open;
inserted = true;
openSize++;
}
else if (PreviousNode == null)
{
node.Previous = null;
node.Next = currNode;
currNode.Previous = node;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
node.Flag = AStarNode.E_AStarFlags.Open;
inserted = true;
openSize++;
headOfOpenList = node;
}
}
else
{
if (NextNode == null)
{
currNode.Next = node;
node.Previous = currNode;
node.Next = null;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
node.Flag = AStarNode.E_AStarFlags.Open;
inserted = true;
openSize++;
}
else
{
currNode = NextNode;
}
}
}
else
{
if (NextNode == null)
{
currNode.Next = node;
node.Previous = currNode;
node.Next = null;
map.SetAStarFlags(node.NodeID, AStarNode.E_AStarFlags.Open);
node.Flag = AStarNode.E_AStarFlags.Open;
inserted = true;
openSize++;
}
else
{
currNode = NextNode;
}
}
}
}
}
