public float DistanceTo(WorldState_GS target, bool no_dist = false) { //Summatory of all the variables distance float total_distance = 0; //Get target world state properties Dictionary <string, Property_GS> target_properties = target.properties; //Iterate target world state properties foreach (KeyValuePair <string, Property_GS> target_property in target_properties) { Property_GS this_property; //Find the property if (_world_state.TryGetValue(target_property.Key, out this_property)) { //Check properties total_distance += this_property.DistanceTo(target_property.Value, no_dist); } //If the property is not found the goal world state is not valid else { Debug.LogError("Goal world state: " + target.name + " defines the variable: " + target_property.Key + " and it is not defined in the current world state!"); return(0); } } //In no goals defined case, also return true, because there's no need to calculate a plan if (target_properties.Count == 0) { Debug.Log("Goal world state: " + target.name + " has no goals defined!"); } return(total_distance); }
public void MixGoals(WorldState_GS mix_source) { foreach (KeyValuePair <string, Property_GS> source_pair in mix_source.properties) { SetGoal(source_pair.Key, source_pair.Value); } }
ActionNode_GS _action = null; //Node Action //Constructors ================ public PlannerNode_GS(float node_g, float node_h, int node_id, int parent_node_id, WorldState_GS resultant_ws, ActionNode_GS node_action) { _g = node_g; _h = node_h; _id = node_id; _parent_id = parent_node_id; _resultant_world_state = resultant_ws; _action = node_action; }
public WorldState_GS(WorldState_GS copy) { _name = copy.name; //Allocate the world state properties dic _world_state = new Dictionary <string, Property_GS>(); //Iterate the clone target and copy all its properties foreach (KeyValuePair <string, Property_GS> copy_pair in copy.properties) { SetGoal(copy_pair.Key, copy_pair.Value); } }
private bool IsInClosed(WorldState_GS target) { //Iterate all the closed nodes and check if anyone has the target world state as resultant world state foreach (KeyValuePair <float, PlannerNode_GS> closed_pair in _closed) { if (closed_pair.Value.resultant_world_state.DistanceTo(target) == 0) { return(true); } } return(false); }
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); }
//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>()); }