/// <summary>
/// Performs one step of the planner.
/// </summary>
public void PerformOneStep()
{
ARAstarNode currentNode = Open.First();
bool openListEmpty = false;
if (((compareKey(currentNode, goalPair.Value) != 1) &&
goalPair.Value.rhs == goalPair.Value.g &&
currentNode.highPriority <= 0 && completePathExists() == true) || nodesExpanded == _maxNumNodesToExpand)
{
nodesExpanded = 0;
if (firstTime)
{
firstTime = false;
}
Open.Sort();
Close.Clear();
Open.clearHighPriority();
Debug.Log("Updating weight. Plan again");
if (inflationFactor >= 1.5f)
{
inflationFactor -= .5f;
}
return;
}
else
{
expandNode(currentNode);
}
Debug.Log("number of nodes expanded : " + nodesExpanded);
if (Incons.Elements().Count > 0)
{
Incons.MoveToOpen(ref Open);
}
}
