tunnel-strategy/src/planner.cpp

#include "planner.hpp"
#include "action.hpp"
#include "character_actor.hpp"
#include "global_world_state.hpp"
#include "state.hpp"
#include "utils/godot_macros.h"
#include <cstdlib>
#include <godot_cpp/templates/pair.hpp>
#include <godot_cpp/variant/utility_functions.hpp>
#include <godot_cpp/templates/hash_set.hpp>
#include <godot_cpp/templates/hash_map.hpp>
#include <godot_cpp/templates/hashfuncs.hpp>
#include <godot_cpp/variant/vector3.hpp>

namespace godot::goap {
typedef HashMap<PlannerNode, PlannerNode, PlannerNodeHasher> NodeNodeMap;
typedef HashMap<PlannerNode, float, PlannerNodeHasher> NodeScoreMap;
typedef HashSet<PlannerNode, PlannerNodeHasher> NodeSet;

void Goal::_bind_methods() {
#define CLASSNAME Goal
    GDPROPERTY(goal_state, Variant::DICTIONARY);
    GDPROPERTY(prerequisites, Variant::DICTIONARY);
}

void Goal::set_goal_state(Dictionary dict) {
    Action::dict_to_property_map(dict, this->goal_state);
}

Dictionary Goal::get_goal_state() const {
    return Action::property_map_to_dict(this->goal_state);
}

void Goal::set_prerequisites(Dictionary dict) {
    Action::dict_to_property_map(dict, this->prerequisites);
}

Dictionary Goal::get_prerequisites() const {
    return Action::property_map_to_dict(this->prerequisites);
}

#undef CLASSNAME // !Goal

PlannerNode PlannerNode::goal_node(WorldState const &goal)  {
    return PlannerNode{goal};
}

PlannerNode PlannerNode::new_node_along(Ref<Action> action) const {
    PlannerNode new_node{
        action->prerequisites,
        action->effects,
        action,
    };
    for(WorldProperty const &prop : this->state)
        new_node.state.insert(prop.key, prop.value);
    for(WorldProperty const &prop : this->open_requirements)
        new_node.open_requirements.insert(prop.key, prop.value);
    return new_node;
}

void Planner::_bind_methods() {
#define CLASSNAME Planner
    GDPROPERTY_HINTED(actions, Variant::ARRAY, PROPERTY_HINT_ARRAY_TYPE, GDRESOURCETYPE(Action));
    GDPROPERTY_HINTED(goals, Variant::ARRAY, PROPERTY_HINT_ARRAY_TYPE, GDRESOURCETYPE(Goal));
}

void Planner::_enter_tree() {
    this->global_world_state = GlobalWorldState::get_singleton();
    this->actor = Object::cast_to<CharacterActor>(this->get_parent());
}

static Vector<Ref<Action>> trace_path(NodeNodeMap &map, PlannerNode &end) {
    Vector<Ref<Action>> edges{};
    PlannerNode node{end};
    while(node.last_edge.is_valid()) {
        edges.push_back(node.last_edge);
        node = map.get(node);
    }
    return edges;
}

Vector<Ref<Action>> Planner::make_plan() {
    // clear cache every planning phase
    this->cached_world_state.clear();
    // select the most desirable goal available
    Ref<Goal> goal = this->select_goal();
    if(!goal.is_valid()) {
        this->plan = {};
        return this->plan;
    }
    // ordered list of all nodes still being considered
    Vector<PlannerNode> open{PlannerNode::goal_node(goal->goal_state)};
    PlannerNode first = open.get(0);
    NodeNodeMap from{}; // mapping states to the previous in the path
    NodeScoreMap dist_traveled{}; // mapping states to the shortest found distance from start
    dist_traveled.insert(first, 0);
    NodeScoreMap best_guess{}; // mapping states to the best guess of the distance to the goal
    best_guess.insert(first, first.open_requirements.size());
    PlannerNode current{}; // state we're checking for neighbours or completion
    while(!open.is_empty()) {
        // current is the top of the ordered list
        current = open.get(0);
        // check if we've reached the goal
        if(current.open_requirements.is_empty()) {
            this->plan = trace_path(from, current);
            return this->plan;
        }
        // current is no longer considered as it cannot be the end
        open.erase(current);
        // find all neighbours of this state
        Vector<PlannerNode> neighbours = this->find_neighbours_of(current);
        for(PlannerNode const& node : neighbours) {
            float const new_dist = dist_traveled.get(current) + 1.f; // unweighed distance traveled to neighbour
            if(!dist_traveled.has(node) || new_dist < dist_traveled.get(node)) {
                // store distances
                dist_traveled[node] = new_dist;
                best_guess[node] = new_dist + node.open_requirements.size();
                from[node] = current;
                int i = open.find(node);
                if(i != -1)
                    open.remove_at(i);
                open.ordered_insert(node);
            }
        }
    }
    UtilityFunctions::push_warning("Failed to find a plan satisfying goal");
    this->plan = {};
    return this->plan;
}

Ref<Goal> Planner::select_goal() {
    for(Ref<Goal> const &goal : this->goals) {
        if(this->can_do(goal))
            return goal;
    }
    return {};
}

Variant Planner::get_world_property(StringName prop_key) {
    if(prop_key.begins_with("g_")) {
        return this->global_world_state->get_world_property(prop_key);
    } else if(this->cached_world_state.has(prop_key)) {
        return this->cached_world_state.get(prop_key);
    } else if(this->actor->has_method("get_" + prop_key)) {
        Variant val = this->actor->call("get_" + prop_key);
        this->cached_world_state.insert(prop_key, val);
        return val;
    } else return nullptr;
}

bool Planner::can_do(Ref<Action> action) {
    for(WorldProperty &prop : action->context_prerequisites)
        if(this->get_world_property(prop.key) != prop.value)
            return false;
    return true;
}

bool Planner::can_do(Ref<Goal> goal) {
    for(WorldProperty const &prop : goal->prerequisites)
        if(this->get_world_property(prop.key) != prop.value)
            return false;
    return true;
}

Vector<PlannerNode> Planner::find_neighbours_of(PlannerNode &node) {
    Vector<PlannerNode> neighbours{};
    for(Ref<Action> const &action : this->find_actions_satisfying(node.open_requirements)) {
        PlannerNode new_node = node.new_node_along(action);
        // remove all satisfied requirements
        for(WorldProperty const &delta : action->effects) {
            if(new_node.open_requirements.has(delta.key)
            && new_node.open_requirements.get(delta.key) == delta.value) {
                new_node.open_requirements.erase(delta.key);
            }
        }
        neighbours.push_back(new_node);
    }
    return neighbours;
}

Vector<Ref<Action>> Planner::find_actions_satisfying(WorldState requirements) {
    Vector<Ref<Action>> found_actions{};
    for(Ref<Action> &act : this->actions) {
        for(WorldProperty &prop : requirements) {
            if(act->effects.has(prop.key)
            && act->effects.get(prop.key) == prop.value
            && this->can_do(act)) {
                found_actions.push_back(act);
            }
        }
    }
    return found_actions;
}

bool Planner::is_action_complete() {
    return this->plan.get(0)->get_is_completed(this->actor);
}

State Planner::get_next_state() {
    if(!this->plan.is_empty())
        this->plan.remove_at(0);
    if(this->plan.is_empty())
        this->plan = this->make_plan();
    if(this->plan.is_empty())
        return State::new_invalid();
    return this->plan.get(0)->apply_state->construct(this);
}

void Planner::set_actions(Array value) {
    this->actions.clear();
    this->actions.resize(value.size());
    for(size_t i{0}; i < value.size(); ++i) {
        Ref<Action> act = value[i];
        if(act.is_valid())
            this->actions.set(i, act);
    }
}

Array Planner::get_actions() const {
    Array array{};
    for(Ref<Action> const &act : this->actions) {
        array.push_back(act);
    }
    return array;
}

void Planner::set_goals(Array value) {
    this->goals.clear();
    this->goals.resize(value.size());
    for(size_t i{0}; i < value.size(); ++i) {
        Ref<Goal> goal = value[i];
        if(goal.is_valid())
            this->goals.set(i, goal);
    }
}

Array Planner::get_goals() const {
    Array array{};
    for(Ref<Goal> const &goal : this->goals) {
        array.push_back(goal);
    }
    return array;
}

bool Planner::add_goal(Ref<Goal> goal) {
    bool can_do = this->can_do(goal);
    this->goals.insert(0, goal);
    return can_do;
}

void Planner::remove_goal(Ref<Goal> goal) {
    this->goals.erase(goal);
}
}