private bool IsInOpen(WorldState_GS target, out PlannerNode_GS found) { //Iterate all the open nodes and check if anyone has the target world state as resultant world state foreach (KeyValuePair <float, List <PlannerNode_GS> > open_pair in _open) { foreach (PlannerNode_GS open_node in open_pair.Value) { if (open_node.resultant_world_state.DistanceTo(target, true) == 0) { found = open_node; return(true); } } } found = null; return(false); }
private void AddToOpen(PlannerNode_GS new_node) { List <PlannerNode_GS> target_list = null; //Try to find the list that stores all the nodes with the same f than the new one if (_open.TryGetValue(new_node.f, out target_list)) { //If the list exists we simply add the new node into it target_list.Add(new_node); } else { //If the new node is the only one with a f value of X we have to allocate a new list for him _open.Add(new_node.f, new List <PlannerNode_GS> { new_node }); } }
private PlannerNode_GS CloseNode() { //Get open nodes listed with the cheapest cost List <PlannerNode_GS> cheapest_nodes = _open[_open.Keys.Min()]; //Get target node PlannerNode_GS target = cheapest_nodes[cheapest_nodes.Count - 1]; //Store the last cheap node in the closed list _closed.Add(target.id, target); //Remove the close node from the open list cheapest_nodes.RemoveAt(cheapest_nodes.Count - 1); //If the removed node was the last we can remove the entire list for this key if (cheapest_nodes.Count == 0) { _open.Remove(_open.Keys.Min()); } //Return the closed node return(_closed[target.id]); }
//Planning Methods ================ public Stack <ActionNode_GS> GeneratePlan(Agent_GS agent) { //First get the world state states //Current WorldState_GS current_world_state = agent.blackboard.GenerateWorldState(); WorldState_GS current_global_world_state = GlobalBlackboard_GS.blackboard.GenerateWorldState(); current_world_state.MixGoals(current_global_world_state); //Goal //Get current as base world state WorldState_GS goal_world_state = new WorldState_GS(current_world_state); //Apply the goals on the current to get the goal world state goal_world_state.MixGoals(agent.goal_world_state); //Check if the current world state and the goal world state coincide float start_goal_distance = current_world_state.DistanceTo(goal_world_state); if (start_goal_distance < ProTools.MIN_PROPERTY_DISTANCE) { Debug.LogWarning("The current world state coincides with the goal world state: " + goal_world_state.name + " !"); return(new Stack <ActionNode_GS>()); } //Allocate plan start node PlannerNode_GS start_node = new PlannerNode_GS(0, start_goal_distance, new_id_number, 0, current_world_state, null); //Clear open and close dictionaries _open.Clear(); _closed.Clear(); //Reset iterations count _current_iterations = 0; //Add start node to the open list _open.Add(start_node.f, new List <PlannerNode_GS> { start_node }); //Iterate open list till there are no nodes to check while (_open.Count > 0) { //Check and update iterations count if (ProTools.ITERATION_LIMIT < _current_iterations) { Debug.LogWarning("Planning generation for agent: " + agent.name + " failed"); break; } else { _current_iterations += 1; } //Current closed node PlannerNode_GS current_node = CloseNode(); //Check if the resultant world state of the current node is the goal world state if (current_node.resultant_world_state.DistanceTo(goal_world_state) < ProTools.MIN_PROPERTY_DISTANCE) { //Allocate a new queue of actions to store the plan Stack <ActionNode_GS> action_plan = new Stack <ActionNode_GS>(); //Enqueue the goal action action_plan.Push(new ActionNode_GS(current_node.action)); //Iterate goal node "childs" to start node using the parent id while (current_node.parent_id != 0) { //Update current node current_node = _closed[current_node.parent_id]; //Check if the node has an action assigned if (current_node.action != null) { //Enqueue the new current node action_plan.Push(new ActionNode_GS(current_node.action)); } } //Flip the generated queue //Return the generated actions queue return(action_plan); } //Iterate all the avaliable actions for (int k = 0; k < agent.action_nodes_num; k++) { if (agent.action_nodes[k].action == null) { continue; } //Scoped action ActionNode_GS scoped_action = agent.action_nodes[k]; //Check if this action is reachable from the current world state if (scoped_action.ValidateWorldState(current_node.resultant_world_state) == false) { //If the current world state is not valid for this actions we discard it and keep iterating the others continue; } //If this action can be executed in the current world state, action effects are applied in the current world state WorldState_GS new_current_world_state = scoped_action.EffectWorldState(current_node.resultant_world_state); //Check if there is an action in the open list that can also reach the new current world state PlannerNode_GS in_open_node = null; if (IsInOpen(new_current_world_state, out in_open_node) == true) { //In true case check if the new node is better if (current_node.g + scoped_action.action_cost > in_open_node.g) { //The old node is better than this new one continue; } //The new node is better than the old, lets update the node data in_open_node.parent_id = current_node.id; in_open_node.g = current_node.g + scoped_action.action_cost; in_open_node.h = new_current_world_state.DistanceTo(goal_world_state); in_open_node.action = scoped_action; } else { //In false case generate a new open node PlannerNode_GS new_node = new PlannerNode_GS(current_node.g + scoped_action.action_cost, new_current_world_state.DistanceTo(goal_world_state), new_id_number, current_node.id, new_current_world_state, scoped_action); //Add the new node to the open list AddToOpen(new_node); } } } //In no plan found case we return an empty plan return(new Stack <ActionNode_GS>()); }