feat: extend Behavior

core/src/Behavior.cpp

@@ -153,3 +153,159 @@ BehaviorNode* BehaviorNode::childAt(int index) const {
     return nullptr;
 }
 
+BehaviorStatus BehaviorNode::tick(BehaviorContext& ctx) {
+    ++visits;
+    if (ctx.traceEnabled) ctx.trace.push_back(nodeName);
+    cachedStatus = onTick(ctx);
+    return cachedStatus;
+}
+
+void BehaviorNode::reset() {
+    std::vector<BehaviorNode*> pending;
+    pending.push_back(this);
+    while (!pending.empty()) {
+        BehaviorNode* node = pending.back();
+        pending.pop_back();
+        node->cachedStatus = BehaviorStatus::Failure;
+        node->onReset();
+        for (int i = 0; i < node->childCount(); ++i) pending.push_back(node->childAt(i));
+    }
+}
+
+ConditionNode::ConditionNode(std::string name, BehaviorPredicate test)
+    : BehaviorNode(std::move(name)), predicate(std::move(test)) {}
+
+BehaviorStatus ConditionNode::onTick(BehaviorContext& ctx) {
+    return predicate(ctx) ? BehaviorStatus::Success : BehaviorStatus::Failure;
+}
+
+ActionNode::ActionNode(std::string name, BehaviorAction fn)
+    : BehaviorNode(std::move(name)), action(std::move(fn)) {}
+
+BehaviorStatus ActionNode::onTick(BehaviorContext& ctx) { return action(ctx); }
+
+BehaviorComposite::BehaviorComposite(std::string name) : BehaviorNode(std::move(name)) {}
+
+BehaviorComposite& BehaviorComposite::add(std::unique_ptr<BehaviorNode> child) {
+    children.push_back(std::move(child));
+    return *this;
+}
+
+BehaviorNode* BehaviorComposite::childAt(int index) const {
+    return children[static_cast<std::size_t>(index)].get();
+}
+
+SelectorNode::SelectorNode(std::string name) : BehaviorComposite(std::move(name)) {}
+
+BehaviorStatus SelectorNode::onTick(BehaviorContext& ctx) {
+    int total = childCount();
+    for (int i = runningChild; i < total; ++i) {
+        BehaviorStatus status = children[static_cast<std::size_t>(i)]->tick(ctx);
+        if (status == BehaviorStatus::Running) {
+            runningChild = i;
+            return BehaviorStatus::Running;
+        }
+        if (status == BehaviorStatus::Success) {
+            runningChild = 0;
+            return BehaviorStatus::Success;
+        }
+    }
+    runningChild = 0;
+    return BehaviorStatus::Failure;
+}
+
+void SelectorNode::onReset() { runningChild = 0; }
+
+SequenceNode::SequenceNode(std::string name) : BehaviorComposite(std::move(name)) {}
+
+BehaviorStatus SequenceNode::onTick(BehaviorContext& ctx) {
+    int total = childCount();
+    for (int i = runningChild; i < total; ++i) {
+        BehaviorStatus status = children[static_cast<std::size_t>(i)]->tick(ctx);
+        if (status == BehaviorStatus::Running) {
+            runningChild = i;
+            return BehaviorStatus::Running;
+        }
+        if (status == BehaviorStatus::Failure) {
+            runningChild = 0;
+            return BehaviorStatus::Failure;
+        }
+    }
+    runningChild = 0;
+    return BehaviorStatus::Success;
+}
+
+void SequenceNode::onReset() { runningChild = 0; }
+
+ParallelNode::ParallelNode(std::string name, int successes, int failures)
+    : BehaviorComposite(std::move(name)),
+      successThreshold(successes),
+      failureThreshold(failures) {}
+
+int ParallelNode::successesSoFar() const {
+    int total = 0;
+    for (std::size_t i = 0; i < latched.size(); ++i) {
+        if (settled[i] && latched[i] == BehaviorStatus::Success) ++total;
+    }
+    return total;
+}
+
+int ParallelNode::failuresSoFar() const {
+    int total = 0;
+    for (std::size_t i = 0; i < latched.size(); ++i) {
+        if (settled[i] && latched[i] == BehaviorStatus::Failure) ++total;
+    }
+    return total;
+}
+
+BehaviorStatus ParallelNode::onTick(BehaviorContext& ctx) {
+    std::size_t total = children.size();
+    if (latched.size() != total) {
+        latched.assign(total, BehaviorStatus::Running);
+        settled.assign(total, false);
+    }
+
+    for (std::size_t i = 0; i < total; ++i) {
+        if (settled[i]) continue;
+        BehaviorStatus status = children[i]->tick(ctx);
+        if (status == BehaviorStatus::Running) continue;
+        latched[i] = status;
+        settled[i] = true;
+    }
+
+    int successes = successesSoFar();
+    int failures = failuresSoFar();
+    if (successes >= successThreshold || failures >= failureThreshold) {
+        for (std::size_t i = 0; i < total; ++i) {
+            if (!settled[i]) children[i]->reset();
+        }
+        latched.assign(total, BehaviorStatus::Running);
+        settled.assign(total, false);
+        return successes >= successThreshold ? BehaviorStatus::Success : BehaviorStatus::Failure;
+    }
+    return BehaviorStatus::Running;
+}
+
+void ParallelNode::onReset() {
+    latched.assign(children.size(), BehaviorStatus::Running);
+    settled.assign(children.size(), false);
+}
+
+DecoratorNode::DecoratorNode(std::string name, std::unique_ptr<BehaviorNode> child)
+    : BehaviorNode(std::move(name)), wrapped(std::move(child)) {}
+
+BehaviorNode* DecoratorNode::childAt(int index) const {
+    (void)index;
+    return wrapped.get();
+}
+
+InverterNode::InverterNode(std::string name, std::unique_ptr<BehaviorNode> child)
+    : DecoratorNode(std::move(name), std::move(child)) {}
+
+BehaviorStatus InverterNode::onTick(BehaviorContext& ctx) {
+    BehaviorStatus status = child().tick(ctx);
+    if (status == BehaviorStatus::Success) return BehaviorStatus::Failure;
+    if (status == BehaviorStatus::Failure) return BehaviorStatus::Success;
+    return BehaviorStatus::Running;
+}
+