diff --git a/libs/util/include/psemek/util/behavior_tree.hpp b/libs/util/include/psemek/util/behavior_tree.hpp index 48af1093..0e0143c2 100644 --- a/libs/util/include/psemek/util/behavior_tree.hpp +++ b/libs/util/include/psemek/util/behavior_tree.hpp @@ -1,10 +1,13 @@ #pragma once +#include + #include #include #include #include #include +#include namespace psemek::util { @@ -64,6 +67,13 @@ namespace psemek::util struct any_node { + any_node() = default; + + explicit operator bool() const + { + return static_cast(impl_); + } + template any_node(Node node) : impl_(new any_node_impl{std::move(node)}) @@ -90,39 +100,63 @@ namespace psemek::util struct updater { - struct node_data - { - any_node node; - std::tuple args; - Time duration; - Time elapsed; - }; + using instance_id = std::uint32_t; - template - void add(Node node, Args ... args) + updater() + : suspended_(suspended_comparator{this}) + {} + + template + instance_id add(Instance instance, Args ... args) { - new_.push_back(node_data{std::move(node), {args...}, 0, 0}); + auto id = instances_.insert(instance_data{std::move(instance), {args...}, 0, std::nullopt}); + new_.push_back(id); + return id; + } + + size_t event(Event const & e, instance_id id) + { + auto & n = instances_[id]; + if (std::apply([&](Args ... args){ return n.tree.event(e, args...); }, n.args)) + { + if (n.suspend && n.suspend->end > time_) + { + suspended_.erase(n.suspend->iterator); + n.suspend->end = time_; + n.suspend->iterator = suspended_.insert(id).first; + } + + return 1; + } + return 0; } template - void event(Event const & e, Filter && filter) + size_t event(Event const & e, Filter && filter) { - for (node_data & n : active_) + size_t count = 0; + + for (auto id : active_) { + auto & n = instances_[id]; if (std::apply(filter, n.args)) - std::apply([&](Args ... args){ n.node.event(e, args...); }, n.args); + count += event(e, id); } - for (node_data & n : suspended_) + for (auto it = suspended_.begin(); it != suspended_.end();) { + auto id = *it++; + auto & n = instances_[id]; if (std::apply(filter, n.args)) - { - if (std::apply([&](Args ... args){ return n.node.event(e, args...); }, n.args)) - { - n.duration = n.elapsed; - } - } + count += event(e, id); } + + return count; + } + + size_t event(Event const & e) + { + return event(e, [](auto const & ...){ return true; }); } struct update_statistics @@ -133,9 +167,11 @@ namespace psemek::util update_statistics update(Time dt) { + time_ += dt; + fill_dt(dt); activate(); - wake_up(dt); + wake_up(); update_statistics result; result.active_count = active_.size(); @@ -147,62 +183,98 @@ namespace psemek::util } private: - std::vector new_; - std::vector active_; - std::vector suspended_; + struct suspended_comparator + { + updater * parent; + + bool operator()(instance_id id1, instance_id id2) const + { + auto tie = [&](instance_id id) -> std::tuple { return {parent->instances_[id].suspend->end, id}; }; + + return tie(id1) < tie(id2); + } + }; + + using suspended_set = std::set; + + struct instance_data + { + any_node tree; + std::tuple args; + Time elapsed; + + struct suspend_data + { + Time start; + Time end; + suspended_set::iterator iterator; + }; + + std::optional suspend; + }; + + flat_list instances_; + Time time_ = 0; + + std::vector new_; + std::vector active_; + std::vector new_active_; + suspended_set suspended_; void fill_dt(Time dt) { - for (auto & n : active_) - n.elapsed = dt; + for (auto id : active_) + instances_[id].elapsed = dt; } void activate() { - for (auto & n : new_) + for (auto id : new_) { + auto & n = instances_[id]; n.elapsed = 0; - std::apply([&](Args ... args){ n.node.start(args...); }, n.args); - active_.push_back(std::move(n)); + std::apply([&](Args ... args){ n.tree.start(args...); }, n.args); + active_.push_back(id); } new_.clear(); } - void wake_up(Time dt) + void wake_up() { - std::vector new_suspended; - for (auto & n : suspended_) + while (!suspended_.empty()) { - n.elapsed += dt; - if (n.elapsed >= n.duration) - active_.push_back(std::move(n)); - else - new_suspended.push_back(std::move(n)); - } + auto id = *suspended_.begin(); + if (instances_[id].suspend->end > time_) + break; + suspended_.erase(suspended_.begin()); - suspended_ = std::move(new_suspended); + active_.push_back(id); + instances_[id].elapsed = time_ - instances_[id].suspend->start; + instances_[id].suspend = std::nullopt; + } } void do_update() { - std::vector new_active; - for (auto & n : active_) + for (auto id : active_) { - auto result = std::apply([&](Args ... args){ return n.node.update(n.elapsed, args...); }, n.args); + auto & n = instances_[id]; + auto result = std::apply([&](Args ... args){ return n.tree.update(n.elapsed, args...); }, n.args); if (auto s = std::get_if(&result)) { - new_.push_back(std::move(n)); + new_.push_back(id); } else if (auto s = std::get_if(&result)) { n.elapsed = 0; - n.duration = s->duration; - suspended_.push_back(std::move(n)); + n.suspend = {time_, time_ + s->duration, suspended_.end()}; + n.suspend->iterator = suspended_.insert(id).first; } else - new_active.push_back(std::move(n)); + new_active_.push_back(id); } - active_ = std::move(new_active); + std::swap(active_, new_active_); + new_active_.clear(); } };