Add new behavior trees library

libs/bt/CMakeLists.txt

@@ -0,0 +1,6 @@
+file(GLOB_RECURSE PSEMEK_BT_HEADERS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "include/*.hpp")
+file(GLOB_RECURSE PSEMEK_BT_SOURCES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "source/*.cpp")
+
+psemek_add_library(psemek-bt ${PSEMEK_BT_HEADERS} ${PSEMEK_BT_SOURCES})
+target_include_directories(psemek-bt PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
+target_link_libraries(psemek-bt PUBLIC psemek-util)

libs/bt/include/psemek/bt/condition.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct condition;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct condition<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		status update(Time, Args ... args) override
+		{
+			return finished{check(args...)};
+		}
+
+		virtual bool check(Args ... args) = 0;
+	};
+
+}

libs/bt/include/psemek/bt/failure.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct failure_node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct failure_node<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		failure_node(node_ptr<tree_type> child)
+			: child_(std::move(child))
+		{}
+
+		void start(Args ... args) override
+		{
+			child_->start(args...);
+		}
+
+		status update(Time dt, Args ... args) override
+		{
+			auto result = child_->update(dt, args...);
+			if (std::get_if<finished>(&result))
+				return finished{false};
+			return result;
+		}
+
+		bool event(Event const & event, Args ... args) override
+		{
+			return child_->event(event, args...);
+		}
+
+	private:
+		node_ptr<tree<Time, Event, Args...>> child_;
+	};
+
+	template <typename Tree>
+	node_ptr<Tree> failure(node_ptr<Tree> child)
+	{
+		return std::make_unique<failure_node<Tree>>(std::move(child));
+	}
+
+}

libs/bt/include/psemek/bt/negate.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+#include <optional>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct negate_node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct negate_node<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		negate_node(node_ptr<tree_type> child)
+			: child_(std::move(child))
+		{}
+
+		void start(Args ... args) override
+		{
+			child_->start(args...);
+		}
+
+		status update(Time dt, Args ... args) override
+		{
+			auto result = child_->update(dt, args...);
+			if (auto f = std::get_if<finished>(&result))
+			{
+				if (f->result)
+					return finished{false};
+				else
+					return finished{true};
+			}
+			return result;
+		}
+
+		bool event(Event const & event, Args ... args) override
+		{
+			return child_->event(event, args...);
+		}
+
+	private:
+		node_ptr<tree<Time, Event, Args...>> child_;
+	};
+
+	template <typename Tree>
+	node_ptr<Tree> negate(node_ptr<Tree> child)
+	{
+		return std::make_unique<negate_node<Tree>>(std::move(child));
+	}
+
+}

libs/bt/include/psemek/bt/node.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <psemek/bt/tree.hpp>
+
+#include <variant>
+#include <memory>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+
+		struct running
+		{};
+
+		struct finished
+		{
+			bool result;
+		};
+
+		struct suspended
+		{
+			Time duration;
+		};
+
+		using status = std::variant<running, finished, suspended>;
+
+		virtual void start(Args ...) {}
+		virtual status update(Time dt, Args ... args) = 0;
+		virtual bool event(Event const &, Args ...) { return false; }
+
+		virtual ~node() {}
+	};
+
+	template <typename Tree>
+	using node_ptr = std::unique_ptr<node<Tree>>;
+
+}

libs/bt/include/psemek/bt/repeat.hpp

@@ -0,0 +1,77 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+#include <optional>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct repeat_node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct repeat_node<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::running;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		repeat_node(node_ptr<tree_type> child, std::optional<std::size_t> max_count)
+			: child_(std::move(child))
+			, max_count_(max_count)
+		{}
+
+		void start(Args ... args) override
+		{
+			child_->start(args...);
+			child_started_ = true;
+			repeat_count_ = 0;
+		}
+
+		status update(Time dt, Args ... args) override
+		{
+			if (!child_started_)
+			{
+				child_->start(args...);
+				child_started_ = true;
+			}
+
+			auto result = child_->update(dt, args...);
+			if (auto f = std::get_if<finished>(&result))
+			{
+				if (!(f->result))
+					return finished{true};
+				else
+				{
+					child_started_ = false;
+					if (++repeat_count_ == max_count_)
+						return finished{false};
+					return running{};
+				}
+			}
+			return result;
+		}
+
+		bool event(Event const & event, Args ... args) override
+		{
+			return child_->event(event, args...);
+		}
+
+	private:
+		node_ptr<tree<Time, Event, Args...>> child_;
+		bool child_started_ = false;
+		std::optional<std::size_t> max_count_;
+		std::size_t repeat_count_ = 0;
+	};
+
+	template <typename Tree>
+	node_ptr<Tree> repeat(node_ptr<Tree> child, std::optional<std::size_t> max_count = std::nullopt)
+	{
+		return std::make_unique<repeat_node<Tree>>(std::move(child), max_count);
+	}
+
+}

libs/bt/include/psemek/bt/retry.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <psemek/bt/repeat.hpp>
+#include <psemek/bt/negate.hpp>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	node_ptr<Tree> retry(node_ptr<Tree> child, std::optional<std::size_t> max_count = std::nullopt)
+	{
+		return repeat<Tree>(negate<Tree>(std::move(child)), max_count);
+	}
+
+}

libs/bt/include/psemek/bt/selector.hpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+#include <vector>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct selector_node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct selector_node<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::running;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		selector_node(std::vector<node_ptr<tree_type>> children)
+			: children_(std::move(children))
+		{}
+
+		void start(Args ...) override
+		{
+			current_index_ = 0;
+			current_started_ = false;
+		}
+
+		status update(Time dt, Args ... args) override
+		{
+			if (current_index_ < children_.size())
+			{
+				if (!current_started_)
+				{
+					children_[current_index_]->start(args...);
+					current_started_ = true;
+				}
+
+				auto result = children_[current_index_]->update(dt, args...);
+				if (auto f = std::get_if<finished>(&result))
+				{
+					if (f->result)
+						return finished{true};
+					++current_index_;
+					current_started_ = false;
+					return running{};
+				}
+				else
+					return result;
+			}
+			return finished{false};
+		}
+
+		bool event(Event const & event, Args ... args) override
+		{
+			if (current_index_ < children_.size())
+				return children_[current_index_]->event(event, args...);
+			return false;
+		}
+
+	private:
+		std::vector<node_ptr<tree<Time, Event, Args...>>> children_;
+		std::size_t current_index_ = 0;
+		bool current_started_ = false;
+	};
+
+	template <typename Tree, typename ... Children>
+	node_ptr<Tree> selector(node_ptr<Tree> child, Children ... other_children)
+	{
+		std::vector<node_ptr<Tree>> children;
+		children.push_back(std::move(child));
+		(children.push_back(std::move(other_children)), ...);
+		return std::make_unique<selector_node<Tree>>(std::move(children));
+	}
+
+}

libs/bt/include/psemek/bt/sequence.hpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+#include <vector>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct sequence_node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct sequence_node<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::running;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		sequence_node(std::vector<node_ptr<tree_type>> children)
+			: children_(std::move(children))
+		{}
+
+		void start(Args ...) override
+		{
+			current_index_ = 0;
+			current_started_ = false;
+		}
+
+		status update(Time dt, Args ... args) override
+		{
+			if (current_index_ < children_.size())
+			{
+				if (!current_started_)
+				{
+					children_[current_index_]->start(args...);
+					current_started_ = true;
+				}
+
+				auto result = children_[current_index_]->update(dt, args...);
+				if (auto f = std::get_if<finished>(&result))
+				{
+					if (!f->result)
+						return finished{false};
+					++current_index_;
+					current_started_ = false;
+					return running{};
+				}
+				else
+					return result;
+			}
+			return finished{true};
+		}
+
+		bool event(Event const & event, Args ... args) override
+		{
+			if (current_index_ < children_.size())
+				return children_[current_index_]->event(event, args...);
+			return false;
+		}
+
+	private:
+		std::vector<node_ptr<tree<Time, Event, Args...>>> children_;
+		std::size_t current_index_ = 0;
+		bool current_started_ = false;
+	};
+
+	template <typename Tree, typename ... Children>
+	node_ptr<Tree> sequence(node_ptr<Tree> child, Children ... other_children)
+	{
+		std::vector<node_ptr<Tree>> children;
+		children.push_back(std::move(child));
+		(children.push_back(std::move(other_children)), ...);
+		return std::make_unique<sequence_node<Tree>>(std::move(children));
+	}
+
+}

libs/bt/include/psemek/bt/success.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+namespace psemek::bt
+{
+
+	template <typename Tree>
+	struct success_node;
+
+	template <typename Time, typename Event, typename ... Args>
+	struct success_node<tree<Time, Event, Args...>>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::finished;
+		using typename node_type::status;
+
+		success_node(node_ptr<tree_type> child)
+			: child_(std::move(child))
+		{}
+
+		void start(Args ... args) override
+		{
+			child_->start(args...);
+		}
+
+		status update(Time dt, Args ... args) override
+		{
+			auto result = child_->update(dt, args...);
+			if (std::get_if<finished>(&result))
+				return finished{true};
+			return result;
+		}
+
+		bool event(Event const & event, Args ... args) override
+		{
+			return child_->event(event, args...);
+		}
+
+	private:
+		node_ptr<tree<Time, Event, Args...>> child_;
+	};
+
+	template <typename Tree>
+	node_ptr<Tree> success(node_ptr<Tree> child)
+	{
+		return std::make_unique<success_node<Tree>>(std::move(child));
+	}
+
+}

libs/bt/include/psemek/bt/tree.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <tuple>
+
+namespace psemek::bt
+{
+
+	template <typename Time, typename Event, typename ... Args>
+	struct tree
+	{
+		using time_type = Time;
+		using event_type = Event;
+		using args_type = std::tuple<Args...>;
+	};
+
+}

libs/bt/include/psemek/bt/wait.hpp

@@ -0,0 +1,73 @@
+#pragma once
+
+#include <psemek/bt/node.hpp>
+
+namespace psemek::bt
+{
+
+	template <typename Tree, typename DurationFn>
+	struct wait_node;
+
+	template <typename Time, typename Event, typename ... Args, typename DurationFn>
+	struct wait_node<tree<Time, Event, Args...>, DurationFn>
+		: node<tree<Time, Event, Args...>>
+	{
+		using tree_type = tree<Time, Event, Args...>;
+		using node_type = node<tree_type>;
+		using typename node_type::finished;
+		using typename node_type::suspended;
+		using typename node_type::status;
+
+		wait_node(DurationFn && duration_fn)
+			: duration_fn_(std::move(duration_fn))
+		{}
+
+		void start(Args ... args) override
+		{
+			remaining_time_ = duration_fn_(args...);
+		}
+
+		status update(Time dt, Args ...) override
+		{
+			remaining_time_ -= dt;
+			if (remaining_time_ <= 0)
+				return finished{true};
+			return suspended{remaining_time_};
+		}
+
+	private:
+		DurationFn duration_fn_;
+		Time remaining_time_ = Time{};
+	};
+
+	namespace detail
+	{
+
+		template <typename Tree, typename DurationFn>
+		struct wait_helper
+		{
+			static node_ptr<Tree> make(DurationFn && duration_fn)
+			{
+				return std::make_unique<wait_node<Tree, DurationFn>>(std::move(duration_fn));
+			}
+		};
+
+		template <typename Time, typename Event, typename ... Args>
+		struct wait_helper<tree<Time, Event, Args...>, Time>
+		{
+			static node_ptr<tree<Time, Event, Args...>> make(Time duration)
+			{
+				auto duration_fn = [duration](Args ...){ return duration; };
+				return std::make_unique<wait_node<tree<Time, Event, Args...>, decltype(duration_fn)>>(std::move(duration_fn));
+			}
+		};
+
+	}
+
+	template <typename Tree, typename DurationFn>
+	node_ptr<Tree> wait(DurationFn && duration_fn)
+	{
+		return detail::wait_helper<Tree, DurationFn>::make(std::move(duration_fn));
+	}
+
+}