Refactor behavior trees: use flat_list & ids for instances, use a binary tree to sort suspended instances

libs/util/include/psemek/util/behavior_tree.hpp

@@ -1,10 +1,13 @@
 #pragma once
 
+#include <psemek/util/flat_list.hpp>
+
 #include <variant>
 #include <tuple>
 #include <memory>
 #include <vector>
 #include <optional>
+#include <set>
 
 namespace psemek::util
 {
@@ -64,6 +67,13 @@ namespace psemek::util
 
 		struct any_node
 		{
+			any_node() = default;
+
+			explicit operator bool() const
+			{
+				return static_cast<bool>(impl_);
+			}
+
 			template <typename Node>
 			any_node(Node node)
 				: impl_(new any_node_impl<Node>{std::move(node)})
@@ -90,39 +100,63 @@ namespace psemek::util
 
 		struct updater
 		{
-			struct node_data
+			using instance_id = std::uint32_t;
-			{
-				any_node node;
-				std::tuple<Args...> args;
-				Time duration;
-				Time elapsed;
-			};
 
-			template <typename Node>
+			updater()
-			void add(Node node, Args ... args)
+				: suspended_(suspended_comparator{this})
+			{}
+
+			template <typename Instance>
+			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 <typename Filter>
-			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))
+						count += event(e, id);
+				}
+
+				return count;
+			}
+
+			size_t event(Event const & e)
 			{
-						if (std::apply([&](Args ... args){ return n.node.event(e, args...); }, n.args))
+				return event(e, [](auto const & ...){ return true; });
-						{
-							n.duration = n.elapsed;
-						}
-					}
-				}
 			}
 
 			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<node_data> new_;
+			struct suspended_comparator
-			std::vector<node_data> active_;
+			{
-			std::vector<node_data> suspended_;
+				updater * parent;
+
+				bool operator()(instance_id id1, instance_id id2) const
+				{
+					auto tie = [&](instance_id id) -> std::tuple<Time, instance_id> { return {parent->instances_[id].suspend->end, id}; };
+
+					return tie(id1) < tie(id2);
+				}
+			};
+
+			using suspended_set = std::set<instance_id, suspended_comparator>;
+
+			struct instance_data
+			{
+				any_node tree;
+				std::tuple<Args...> args;
+				Time elapsed;
+
+				struct suspend_data
+				{
+					Time start;
+					Time end;
+					suspended_set::iterator iterator;
+				};
+
+				std::optional<suspend_data> suspend;
+			};
+
+			flat_list<instance_data, instance_id> instances_;
+			Time time_ = 0;
+
+			std::vector<instance_id> new_;
+			std::vector<instance_id> active_;
+			std::vector<instance_id> new_active_;
+			suspended_set suspended_;
 
 			void fill_dt(Time dt)
 			{
-				for (auto & n : active_)
+				for (auto id : active_)
-					n.elapsed = dt;
+					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);
+					std::apply([&](Args ... args){ n.tree.start(args...); }, n.args);
-					active_.push_back(std::move(n));
+					active_.push_back(id);
 				}
 				new_.clear();
 			}
 
-			void wake_up(Time dt)
+			void wake_up()
 			{
-				std::vector<node_data> new_suspended;
+				while (!suspended_.empty())
-				for (auto & n : suspended_)
 				{
-					n.elapsed += dt;
+					auto id = *suspended_.begin();
-					if (n.elapsed >= n.duration)
+					if (instances_[id].suspend->end > time_)
-						active_.push_back(std::move(n));
+						break;
-					else
+					suspended_.erase(suspended_.begin());
-						new_suspended.push_back(std::move(n));
-				}
 
-				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<node_data> new_active;
+				for (auto id : active_)
-				for (auto & n : 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<finished>(&result))
 					{
-						new_.push_back(std::move(n));
+						new_.push_back(id);
 					}
 					else if (auto s = std::get_if<suspended>(&result))
 					{
 						n.elapsed = 0;
-						n.duration = s->duration;
+						n.suspend = {time_, time_ + s->duration, suspended_.end()};
-						suspended_.push_back(std::move(n));
+						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();
 			}
 		};