206 lines
7.9 KiB
C++
206 lines
7.9 KiB
C++
#include "planner.hpp"
|
|
#include "action_db.hpp"
|
|
#include "godot_cpp/classes/engine.hpp"
|
|
#include "godot_cpp/classes/global_constants.hpp"
|
|
#include "godot_cpp/templates/hashfuncs.hpp"
|
|
#include "utils/godot_macros.hpp"
|
|
#include <cstdint>
|
|
#include <godot_cpp/variant/utility_functions.hpp>
|
|
|
|
namespace goap {
|
|
WorldStateNode::WorldStateNode(WorldState const &goal, ActorWorldState *context)
|
|
: state{}, open_requirements{goal}, context{context} {}
|
|
|
|
WorldStateNode::WorldStateNode(WorldStateNode const &last_state, Action const *last_action)
|
|
: state{last_state.state}
|
|
, open_requirements{last_state.open_requirements}
|
|
, last_action{last_action}
|
|
, context{last_state.context} {
|
|
for(WorldProperty const &req : last_action->get_required())
|
|
this->open_requirements[req.key] = req.value;
|
|
for(WorldProperty const &effect : last_action->get_effects())
|
|
if(this->open_requirements.has(effect.key))
|
|
this->state[effect.key] = effect.value;
|
|
}
|
|
|
|
int WorldStateNode::requirements_unmet() const {
|
|
int requirements = this->open_requirements.size();
|
|
for(WorldProperty const &req : this->open_requirements) {
|
|
if(this->state.has(req.key) && this->state[req.key] == req.value) {
|
|
--requirements;
|
|
} else if(this->context->check_property_match(req)) {
|
|
--requirements;
|
|
}
|
|
}
|
|
return requirements;
|
|
}
|
|
|
|
uint32_t WorldStateNodeHasher::hash(goap::WorldStateNode const &state) {
|
|
uint32_t hash{1};
|
|
for(goap::WorldProperty const &prop : state.state) {
|
|
hash = gd::hash_murmur3_one_32(prop.key.hash(), hash);
|
|
hash = gd::hash_murmur3_one_32(prop.value.hash(), hash);
|
|
}
|
|
return gd::hash_fmix32(hash);
|
|
}
|
|
|
|
bool operator==(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
|
|
if(lhs.state.size() != rhs.state.size())
|
|
return false;
|
|
for(goap::WorldProperty const &prop_lhs : lhs.state) {
|
|
if((!rhs.state.has(prop_lhs.key)) || rhs.state[prop_lhs.key] != prop_lhs.value)
|
|
return false;
|
|
}
|
|
for(goap::WorldProperty const &prop_rhs : rhs.state) {
|
|
if((!lhs.state.has(prop_rhs.key)) || lhs.state[prop_rhs.key] != prop_rhs.value)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool operator!=(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
|
|
return !(lhs == rhs);
|
|
}
|
|
|
|
bool operator<(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
|
|
return lhs.state.size() < rhs.state.size();
|
|
}
|
|
|
|
bool operator<=(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
|
|
return lhs.state.size() <= rhs.state.size();
|
|
}
|
|
|
|
bool operator>(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
|
|
return lhs.state.size() > rhs.state.size();
|
|
}
|
|
|
|
bool operator>=(goap::WorldStateNode const &lhs, goap::WorldStateNode const &rhs) {
|
|
return lhs.state.size() >= rhs.state.size();
|
|
}
|
|
|
|
void Planner::_bind_methods() {
|
|
#define CLASSNAME Planner
|
|
GDPROPERTY_HINTED(actions_inspector, gd::Variant::ARRAY, gd::PROPERTY_HINT_ARRAY_TYPE, ActionDB::get_array_hint());
|
|
}
|
|
|
|
void Planner::_enter_tree() { GDGAMEONLY();
|
|
this->world_state = this->get_node<ActorWorldState>("../ActorWorldState");
|
|
}
|
|
|
|
Plan Planner::plan_for_goal(gd::Ref<Goal> goal) {
|
|
// exit if goal reference is invalid or goal is already completed
|
|
if(!goal.is_valid()) {
|
|
gd::UtilityFunctions::push_warning(this->get_path(), " attempt to plan for invalid goal");
|
|
return {};
|
|
}
|
|
// The node representing the start of the search and the goal of the plan.
|
|
WorldStateNode goal_node{goal->desired_state, this->world_state};
|
|
if(goal_node.requirements_unmet() == 0) {
|
|
return {};
|
|
}
|
|
gd::Vector<WorldStateNode> open{goal_node};
|
|
NodeMap from{}; // mapping nodes to the next node in the plan
|
|
NodeScoreMap best_path_cost{}; // map nodes to the cost of the best path found so far
|
|
NodeScoreMap heuristic_cost{}; // map nodes to their approximate cost to get to their goal
|
|
|
|
// insert with the goal node into the cost maps,
|
|
// path scored 0 as it's the goal
|
|
best_path_cost.insert(goal_node, 0.f);
|
|
heuristic_cost.insert(goal_node, goal_node.requirements_unmet());
|
|
|
|
WorldStateNode current{goal_node};
|
|
// repeat until current has no traversible edges
|
|
while(!open.is_empty()) {
|
|
current = open.get(0);
|
|
// return found path when all requirements are met
|
|
if(current.requirements_unmet() == 0)
|
|
return this->unroll_plan(current, from);
|
|
open.remove_at(0); // pop current off the top of the open set
|
|
// get all actions that contribute to the open requirements of `current`
|
|
gd::Vector<Action const *> edges{this->get_neighbours(current)};
|
|
for(Action const * action : edges) {
|
|
// construct the node produced by traversing edge
|
|
WorldStateNode node_through{current, action};
|
|
// calculate the cost of getting here from current through action.
|
|
float const path_cost{best_path_cost.get(current) + 1.f};
|
|
// store only if path through `current` represents a new fastest way to get to the new node
|
|
if(!best_path_cost.has(node_through) || best_path_cost.get(node_through) >= path_cost) {
|
|
// update cheapest route found
|
|
best_path_cost[node_through] = path_cost;
|
|
// approximate cost to goal
|
|
heuristic_cost[node_through] = path_cost + node_through.requirements_unmet();
|
|
// store path to get to this point
|
|
from[node_through] = current;
|
|
// (re-) insert node according to priority based on unmet requirements
|
|
if(open.has(node_through))
|
|
open.erase(node_through);
|
|
open.ordered_insert(node_through);
|
|
}
|
|
}
|
|
}
|
|
|
|
gd::UtilityFunctions::push_warning("failed to find plan for goal ", goal->get_path());
|
|
return {};
|
|
}
|
|
|
|
void Planner::set_actions_inspector(gd::Array array) {
|
|
this->actions.clear();
|
|
for(size_t i = 0; i < array.size(); ++i) {
|
|
Action const *action = int(array[i]) < 0 ? nullptr : ActionDB::get_action(array[i]);
|
|
if(action != nullptr && !this->actions.has(action)) {
|
|
this->actions.push_back(action);
|
|
} else if(gd::Engine::get_singleton()->is_editor_hint() && !this->actions.has(nullptr)) {
|
|
this->actions.push_back(nullptr);
|
|
}
|
|
}
|
|
}
|
|
|
|
gd::Array Planner::get_actions_inspector() const {
|
|
gd::Array array{};
|
|
for(Action const *action : this->actions)
|
|
array.push_back(action == nullptr ? -1 : action->get_id());
|
|
return array;
|
|
}
|
|
|
|
gd::Vector<Action const *> &Planner::get_actions() {
|
|
return this->actions;
|
|
}
|
|
|
|
gd::Vector<Action const *> Planner::get_neighbours(WorldStateNode const &from) const {
|
|
gd::Vector<Action const *> retval{};
|
|
for(Action const *action : this->actions) {
|
|
if(!action->is_possible(from.context) || !this->does_action_contribute(action, from))
|
|
continue;
|
|
retval.push_back(action);
|
|
}
|
|
return retval;
|
|
}
|
|
|
|
bool Planner::does_action_contribute(Action const *action, WorldStateNode const &node) const {
|
|
WorldState const &effects = action->get_effects();
|
|
for(WorldProperty const &goal : node.open_requirements) {
|
|
if(node.state.has(goal.key) && goal.value == node.state.get(goal.key))
|
|
continue; // requirement already met, don't count as requiring contribution
|
|
if(effects.has(goal.key) && effects[goal.key] == goal.value)
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Plan Planner::unroll_plan(WorldStateNode const &start_node, NodeMap const &from) {
|
|
Plan plan{};
|
|
WorldStateNode node{start_node};
|
|
#if DEBUG_ENABLED
|
|
gd::UtilityFunctions::print("plan (", this->get_path(), "):");
|
|
#endif
|
|
while(node.last_action != nullptr) {
|
|
plan.push_back(node.last_action);
|
|
#if DEBUG_ENABLED
|
|
gd::UtilityFunctions::print(" (", plan.size(), ") ", node.last_action->get_class());
|
|
#endif
|
|
node = from.get(node);
|
|
}
|
|
return plan;
|
|
}
|
|
}
|