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