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