tunnel-strategy/src/planner.cpp

#include "planner.hpp"
#include "action.hpp"
#include "global_world_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> FromMap;
typedef HashMap<PlannerNode, float, PlannerNodeHasher> ScoreMap;
typedef HashSet<PlannerNode, PlannerNodeHasher> NodeSet;

void Goal::_bind_methods() {
#define CLASSNAME Goal
    GDPROPERTY(goal_state, 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);
}

#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));
    GDFUNCTION_ARGS(gdscript_make_plan, "goal");
}

void Planner::_ready() {
    this->global_world_state = GlobalWorldState::get_singleton();
}

void Planner::_process(double delta_time) {
    this->cached_world_state.clear();
}

static Vector<Ref<Action>> trace_path(FromMap &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;
}

Array Planner::gdscript_make_plan(Ref<Goal> goal) {
    Vector<Ref<Action>> plan = this->make_plan(goal);
    Array out{};
    int i{0};
    UtilityFunctions::print("plan len: ", plan.size());
    for(Ref<Action> const &action : plan) {
        out.push_back(action);
        UtilityFunctions::print("plan[", i++, "]: ", this->actions.find(action)); 
    }
    return out;
}

Vector<Ref<Action>> Planner::make_plan(Ref<Goal> goal) {
    // ordered list of all nodes still being considered
    Vector<PlannerNode> open{PlannerNode::goal_node(goal->goal_state)};
    PlannerNode first = open.get(0);
    FromMap from{}; // mapping states to the previous in the path
    ScoreMap dist_traveled{}; // mapping states to the shortest found distance from start
    dist_traveled.insert(first, 0);
    ScoreMap 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())
            return trace_path(from, current);
        // 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);
            }
        }
    }
    return {};
}

Variant Planner::get_world_property(StringName prop_key) {
    if(world_state_override.has(prop_key))
        return world_state_override.get(prop_key);
    if(prop_key.begins_with("g_"))
        return this->global_world_state->get_world_property(prop_key);
    if(this->cached_world_state.has(prop_key))
        return this->cached_world_state.get(prop_key);
    if(this->has_method("get_" + prop_key)) {
        Variant val = this->call(prop_key);
        this->cached_world_state.insert(prop_key, val);
        return val;
    }
    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;
}

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

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