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