diff --git a/core/src/Behavior.cpp b/core/src/Behavior.cpp index e2c5fbe..72e88c5 100644 --- a/core/src/Behavior.cpp +++ b/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 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 child) { + children.push_back(std::move(child)); + return *this; +} + +BehaviorNode* BehaviorComposite::childAt(int index) const { + return children[static_cast(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(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(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 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 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; +} +