public INode <T> Run(INode <T> start, T goal, int maxIterations = 100, bool earlyExit = true, bool clearNodes = true, bool debugPlan = false) { this.debugPlan = debugPlan; frontier.Clear(); stateToNode.Clear(); explored.Clear(); if (clearNodes) { ClearNodes(); createdNodes.Add(start); } frontier.Enqueue(start, start.GetCost()); DebugPlan(start, null); var iterations = 0; while ((frontier.Count > 0) && (iterations < maxIterations) && (frontier.Count + 1 < frontier.MaxSize)) { var node = frontier.Dequeue(); if (node.IsGoal(goal)) { ReGoapLogger.Log("[Astar] Success iterations: " + iterations); EndDebugPlan(node); return(node); } explored[node.GetState()] = node; foreach (var child in node.Expand()) { iterations++; if (clearNodes) { createdNodes.Add(child); } if (earlyExit && child.IsGoal(goal)) { ReGoapLogger.Log("[Astar] (early exit) Success iterations: " + iterations); EndDebugPlan(child); return(child); } var childCost = child.GetCost(); var state = child.GetState(); if (explored.ContainsKey(state)) { continue; } INode <T> similiarNode; stateToNode.TryGetValue(state, out similiarNode); if (similiarNode != null) { if (similiarNode.GetCost() > childCost) { frontier.Remove(similiarNode); } else { break; } } DebugPlan(child, node); //Utilities.ReGoapLogger.Log(string.Format(" Enqueue frontier: {0}, cost: {1}", child.Name, childCost)); frontier.Enqueue(child, childCost); stateToNode[state] = child; } } ReGoapLogger.LogWarning("[Astar] failed."); EndDebugPlan(null); return(null); }
public INode <T> Run(INode <T> start, T goal, int maxIterations = 100, bool earlyExit = true, bool clearNodes = true, bool debugPlan = false) { _debugPlan = debugPlan; frontier.Clear(); stateToNode.Clear(); explored.Clear(); if (clearNodes) { ClearNodes(); createdNodes.Add(start); } frontier.Enqueue(start, start.GetCost()); _DebugPlan(start, null); var iterations = 0; while ((frontier.Count > 0) && (iterations < maxIterations) && (frontier.Count + 1 < frontier.MaxSize)) { var node = frontier.Dequeue(); Utilities.ReGoapLogger.Log(string.Format("\n++++Explored action: {0}({3}), state ({1})\n goal ({2})\n effect: ({4})", node.Name, node.GetState(), node.GoalString, node.GetCost(), node.EffectString)); if (node.IsGoal(goal)) { ReGoapLogger.Log("[Astar] Success iterations: " + iterations); _EndDebugPlan(node); return(node); } explored[node.GetState()] = node; foreach (var child in node.Expand()) { iterations++; if (clearNodes) { createdNodes.Add(child); } if (earlyExit && child.IsGoal(goal)) { ReGoapLogger.Log("[Astar] (early exit) Success iterations: " + iterations); _EndDebugPlan(child); return(child); } var childCost = child.GetCost(); var state = child.GetState(); if (explored.ContainsKey(state)) { continue; } INode <T> similiarNode; stateToNode.TryGetValue(state, out similiarNode); if (similiarNode != null) { if (similiarNode.GetCost() > childCost) { frontier.Remove(similiarNode); } else { break; } } _DebugPlan(child, node); Utilities.ReGoapLogger.Log(string.Format(" Enqueue frontier: {0}, cost: {1}", child.Name, childCost)); frontier.Enqueue(child, childCost); stateToNode[state] = child; } } string failReason = (frontier.Count <= 0 ? "Depleted Search Space. " : "") + ((iterations >= maxIterations) ? "Too many iter: " + iterations : "") + (frontier.Count + 1 >= frontier.MaxSize ? "FrontierQueue too large: " + frontier.MaxSize : ""); ReGoapLogger.LogWarning("[Astar] failed. " + failReason); _EndDebugPlan(null); return(null); }
public INode <T> Run(INode <T> start, T goal, int maxIterations = 100, bool earlyExit = true, bool clearNodes = true) { frontier.Clear(); stateToNode.Clear(); explored.Clear(); if (clearNodes) { ClearNodes(); createdNodes.Add(start); } frontier.Enqueue(start, start.GetCost()); var iterations = 0; while ((frontier.Count > 0) && (iterations < maxIterations) && (frontier.Count + 1 < frontier.MaxSize)) { var node = frontier.Dequeue(); if (node.IsGoal(goal)) { return(node); } explored[node.GetState()] = node; foreach (var child in node.Expand()) { iterations++; if (clearNodes) { createdNodes.Add(child); } if (earlyExit && child.IsGoal(goal)) { return(child); } var childCost = child.GetCost(); var state = child.GetState(); if (explored.ContainsKey(state)) { continue; } INode <T> similiarNode; stateToNode.TryGetValue(state, out similiarNode); if (similiarNode != null) { if (similiarNode.GetCost() > childCost) { frontier.Remove(similiarNode); } else { break; } } frontier.Enqueue(child, childCost); stateToNode[state] = child; } } return(null); }