private InternalNode <T> GetInternal(T node)
{
if (!_internalNodes.ContainsKey(node))
{
_internalNodes[node] = new InternalNode <T>
{
Data = node
};
}
return(_internalNodes[node]);
}
public AStarResult Step()
{
if (_steps > MaxSteps)
{
return(AStarResult.Failed);
}
_steps += 1;
if (_openList.Count == 0)
{
return(AStarResult.Failed);
}
var parent = _openList.Min();
parent.Closed = true;
_openList.Remove(parent);
if (parent.Data.Equivalent(Goal))
{
_goalNode = parent;
return(AStarResult.Found);
}
parent.Closed = true;
foreach (var child in parent.Data.Neighbors)
{
if (child == null)
{
continue;
}
var cost = parent.GivenCost + child.GetTraversalCost(parent.Data);
var childNode = GetInternal(child);
if (childNode.Open || childNode.Closed)
{
if (childNode.GivenCost <= cost)
{
continue;
}
var heuristic = childNode.TotalCost - childNode.GivenCost;
childNode.TotalCost = cost + heuristic;
if (childNode.Closed)
{
childNode.Closed = false;
_openList.Add(childNode);
}
}
else
{
var heuristic = child.GetHeuristic(Goal);
childNode.TotalCost = cost + heuristic;
_openList.Add(childNode);
}
childNode.Parent = parent;
childNode.GivenCost = cost;
childNode.Open = true;
}
return(AStarResult.Working);
}
public int CompareTo(InternalNode <T> other)
{
return(TotalCost.CompareTo(other.TotalCost));
}