private SearchNode Search(SearchNode node, float valueDepth, ref float nextThreshhold)
{
CurrentNode = node;
if (Config.IsGoalFunc(node))
{
return(node);
}
OnNodeExpanded();
var actions = Config.ActionsListFunc(node);
foreach (var action in actions)
{
var childNode = new SearchNode
{
Parent = node,
Data = Config.ActionApplierFunc(node, action),
Action = action,
Cost = node.Cost + Config.ActionCostFunc(node, action),
};
childNode.EstimatedRemainingCost = Config.RoundEstimatedRemainingCost
? (int)(Config.HeuristicFunc(childNode.Data) * Config.HeuristicWeighting)
: (Config.HeuristicFunc(childNode.Data) * Config.HeuristicWeighting);
OnNodeGenerated();
if (childNode.EstimatedTotalCost <= valueDepth)
{
var result = Search(childNode, valueDepth, ref nextThreshhold);
if (result != null)
{
return(result);
}
}
else
{
if (childNode.EstimatedTotalCost < nextThreshhold)
{
nextThreshhold = childNode.EstimatedTotalCost;
}
}
}
return(null);
}
public List <SearchNode> Search(TData root, float costLimit = float.MaxValue)
{
NextCostLimitThreshhold = float.MaxValue;
var priorityQueue = new PriorityQueueHeap <SearchNode>();
// Start at root
var rootNode = new SearchNode
{
Data = root,
Action = default(TAction),
Parent = null,
Cost = 0,
EstimatedRemainingCost = Config.HeuristicFunc(root),
};
priorityQueue.Insert(rootNode.EstimatedTotalCost, rootNode);
while (!priorityQueue.IsEmpty)
{
CurrentNode = priorityQueue.ExtractTop();
if (Config.IsGoalFunc(CurrentNode))
{
return(ReconstructPath(CurrentNode));
}
var actions = Config.ActionsListFunc(CurrentNode);
foreach (var action in actions)
{
var newNode = new SearchNode
{
Parent = CurrentNode,
Data = Config.ActionApplierFunc(CurrentNode, action),
Action = action,
Cost = CurrentNode.Cost + Config.ActionCostFunc(CurrentNode, action),
};
newNode.EstimatedRemainingCost = Config.HeuristicFunc(newNode.Data);
if (newNode.Cost <= costLimit)
{
priorityQueue.Insert(newNode.EstimatedTotalCost, newNode);
OnNodeGenerated();
}
else
{
if (newNode.Cost < NextCostLimitThreshhold)
{
NextCostLimitThreshhold = newNode.Cost;
}
}
}
OnNodeExpanded();
NodesRetainedCount = priorityQueue.Count;
}
return(null);
}