Refactor behaviour tree decorators

2024-01-10 21:17:39 +01:00
parent b09f1d4ca9
commit 93b665b290
2 changed files with 163 additions and 115 deletions
--- a/engine/breeze/ai/behaviour_tree.c
+++ b/engine/breeze/ai/behaviour_tree.c
@@ -253,59 +253,104 @@ void bzBTDestroyState(BzBTState *state) {
    bzMemSet(state, 0, sizeof(*state));
 }

-void bzBTStateAppend(BzBTState *state, BzBTNodeState *nodeState) {
+typedef struct BzBTExecState {
+    BzBTNodeState *first;
+    BzBTNodeState *last;
+    void *userData;
+} BzBTExecState;
+typedef struct BzBTExecStatus {
+    BzBTStatus    status;
+    BzBTExecState state;
+} BzBTExecStatus;
+
+BzBTNodeState *execStatePopFront(BzBTExecState *state) {
+    BZ_ASSERT(state->first);
+    BzBTNodeState *popped = state->first;
+
+    if (state->first)
+        state->first = state->first->next;
+    if (state->first == NULL)
+        state->last = NULL;
+
+    popped->prev = NULL;
+    popped->next = NULL;
+    return popped;
+}
+void execStatePushBack(BzBTExecState *state, BzBTNodeState *nodeState) {
+    BZ_ASSERT(state);
+
    nodeState->next = NULL;
-    nodeState->prev = state->_last;
-    if (state->_last)
-        state->_last->next = nodeState;
-    else
-        state->_first = nodeState;
-    state->_last = nodeState;
+    nodeState->prev = state->last;
+
+    if (state->last) {
+        state->last->next = nodeState;
+    } else {
+        state->first = nodeState;
+        state->last = nodeState;
+    }
+    state->last = nodeState;
 }
-void bzBTStatePop(BzBTState *state, BzBTNodeState *nodeState) {
-    if (state->_first == nodeState) state->_first = nodeState->next;
-    if (state->_last == nodeState) state->_last = nodeState->prev;
-    BzBTNodeState *next = nodeState->next;
-    BzBTNodeState *prev = nodeState->prev;
-    if (nodeState->prev)
-        nodeState->prev->next = next;
-    if (nodeState->next)
-        nodeState->next->prev = prev;
-    nodeState->next = NULL;
-    nodeState->prev = NULL;
+void execStateClear(BzBTExecState *state, BzObjectPool *pool) {
+    BzBTNodeState *pState = state->first;
+    while (pState) {
+        BzBTNodeState *next = pState->next;
+        bzObjectPoolRelease(pool, pState);
+        pState = next;
+    }
+    state->first = NULL;
+    state->last = NULL;
 }
-void bzBTStateRenew(BzBTState *oldState, BzBTState *newState, BzBTNodeState *nodeState) {
-    // Pop nodeState and transfer it to the back
-    bzBTStatePop(oldState, nodeState);
-    bzBTStateAppend(newState, nodeState);
-}
-BzBTNodeState *bzBTStatePool(BzBTState *state, const BzBTNode *node) {
-    BzBTNodeState *nodeState = bzObjectPool(state->nodeStatePool);
-    nodeState->next = NULL;
-    nodeState->prev = NULL;
-    nodeState->node = node;
-    return nodeState;
-}
-void bzBTStateRelease(BzBTState *state, BzBTNodeState *nodeState) {
-    bzBTStatePop(state, nodeState);
-    bzObjectPoolRelease(state->nodeStatePool, nodeState);
+void execStateMerge(BzBTExecState *state, BzBTExecState *toMerge) {
+    BzBTNodeState *pState = toMerge->first;
+    while (pState) {
+        BzBTNodeState *next = pState->next;
+        execStatePushBack(state, pState);
+        pState = next;
+    }
+
+    toMerge->first = NULL;
+    toMerge->last = NULL;
 }

-bool nodeMatchesState(const BzBTNode *node, const BzBTNodeState *state) {
-    return bzBTNodeMatchesState(node, state);
+
+/**
+ *
+ * @param node
+ * @param state
+ * @return
+ */
+bool nodeMatchesState(const BzBTNode *node, const BzBTExecState *state) {
+    return state->first && state->first->node == node;
 }
-BzBTNodeState *getNextNodeState(const BzBTNode *node, BzBTNodeState *nodeState) {
-    if (nodeState && nodeMatchesState(node, nodeState))
-        return nodeState->next;
-    return nodeState;
+/**
+ * Return BzBTNodeState for given node, if node matches first state node.
+ */
+static BzBTNodeState *getNodeState(const BzBTNode *node, BzBTExecState *state) {
+    if (nodeMatchesState(node, state)) {
+        return execStatePopFront(state);
+    }
+    return NULL;
+}
+static BzBTNodeState *ensureValidNodeState(const BzBTNode *node, BzObjectPool *pool, BzBTNodeState *existing) {
+    if (existing)
+        return existing;
+
+    existing = bzObjectPool(pool);
+    existing->node = node;
+    existing->prev = NULL;
+    existing->next = NULL;
+    bzMemSet(&existing->as, 0, sizeof(existing->as));
+    return existing;
+}
+static void releaseNodeState(BzObjectPool *pool, BzBTNodeState *nodeState) {
+    bzObjectPoolRelease(pool, nodeState);
 }

-static inline BzBTStatus bzBTExecuteNode(const BzBTNode *node, f32 dt,
-                                         BzBTNodeState *nodeState,
-                                         BzBTState *oldState, BzBTState *newState);
-static inline BzBTStatus bzBTExecuteComposite(const BzBTNode *node, f32 dt,
-                                              BzBTNodeState *nodeState,
-                                              BzBTState *oldState, BzBTState *newState) {
+static inline BzBTExecStatus bzBTExecuteNode(const BzBTNode *node, f32 dt,
+                                             BzBTExecState *nodeState, BzObjectPool *statePool);
+#if 0
+static inline BzBTExecStatus bzBTExecuteComposite(const BzBTNode *node, f32 dt,
+                                                  BzBTExecState *nodeState, BzObjectPool *statePool) {
    BzBTNodeState *nextState = getNextNodeState(node, nodeState);
    BzBTNode *start = node->first;
    bool isParallel = node->type == BZ_BT_COMP_PARALLEL_SEQUENCE ||
@@ -395,107 +440,111 @@ static inline BzBTStatus bzBTExecuteComposite(const BzBTNode *node, f32 dt,
    }
    return status;
 }
-static inline BzBTStatus bzBTExecuteDecorator(const BzBTNode *node, f32 dt,
-                                              BzBTNodeState *nodeState,
-                                              BzBTState *oldState, BzBTState *newState) {
+#endif
+static inline BzBTExecStatus bzBTExecuteDecorator(const BzBTNode *node, f32 dt,
+                                                  BzBTExecState *state, BzObjectPool *statePool) {
    // Ensure decorator has only one child, if any
    BZ_ASSERT(!node->first || node->first == node->last);
-    BzBTNodeState *nextState = getNextNodeState(node, nodeState);
+
+    BzBTNodeState *nodeState = getNodeState(node, state);
+
+    BzBTExecStatus retStatus = { .status = BZ_BT_ERROR };

    switch (node->type) {
        case BZ_BT_DECOR_REPEAT:
-            if (!nodeMatchesState(node, nodeState)) {
-                BzBTNodeState *newNodeState = bzBTStatePool(oldState, node);
-                newNodeState->as.repeat.iter = 0;
-                bzBTStateAppend(newState, newNodeState);
-                nodeState = newNodeState;
-            } else {
-                bzBTStateRenew(oldState, newState, nodeState);
-            }
+            nodeState = ensureValidNodeState(node, statePool, nodeState);
+            execStatePushBack(&retStatus.state, nodeState);
            break;
        case BZ_BT_DECOR_DELAY:
-            if (!nodeMatchesState(node, nodeState)) {
-                BzBTNodeState *newNodeState = bzBTStatePool(oldState, node);
-                newNodeState->as.delay.elapsed = dt;
-                bzBTStateAppend(newState, newNodeState);
-                return BZ_BT_RUNNING;
-            }
+            nodeState = ensureValidNodeState(node, statePool, nodeState);
            nodeState->as.delay.elapsed += dt;
+            execStatePushBack(&retStatus.state, nodeState);
            if (nodeState->as.delay.elapsed < node->as.delay.ms) {
-                bzBTStateRenew(oldState, newState, nodeState);
-                return BZ_BT_RUNNING;
+                retStatus.status = BZ_BT_RUNNING;
+                return retStatus;
            }
+            BZ_ASSERT(nodeState == execStatePopFront(&retStatus.state));
+            releaseNodeState(statePool, nodeState);
            break;
        default:
            break;
    }

    // Implicit success, if no children are present
-    BzBTStatus inStatus = BZ_BT_SUCCESS;
+    BzBTExecStatus inStatus = { .status = BZ_BT_SUCCESS };
    if (node->first)
-        inStatus = bzBTExecuteNode(node->first, dt, nextState, oldState, newState);
+        inStatus = bzBTExecuteNode(node->first, dt, state, statePool);

    // Propagate ERROR, RUNNING up
-    if (inStatus == BZ_BT_ERROR)
-        return BZ_BT_ERROR;
-    if (inStatus == BZ_BT_RUNNING)
-        return BZ_BT_RUNNING;
+    if (inStatus.status == BZ_BT_ERROR) {
+        execStateClear(&inStatus.state, statePool);
+        execStateClear(&retStatus.state, statePool);
+        retStatus.status = BZ_BT_ERROR;
+        return retStatus;
+
+    }
+    if (inStatus.status == BZ_BT_RUNNING) {
+        execStateMerge(&retStatus.state, &inStatus.state);
+        retStatus.status = BZ_BT_RUNNING;
+        return retStatus;
+    }
+    BZ_ASSERT(inStatus.state.first == NULL);

-    BzBTStatus status = BZ_BT_ERROR;
    switch (node->type) {
        case BZ_BT_DECOR_DUMMY:
        case BZ_BT_DECOR_DELAY: // Delay already handled
-            status = inStatus;
+            retStatus.status = inStatus.status;
            break;
        case BZ_BT_DECOR_SUCCESS:
-            status = BZ_BT_SUCCESS;
+            retStatus.status = BZ_BT_SUCCESS;
            break;
        case BZ_BT_DECOR_FAIL:
-            status = BZ_BT_FAIL;
+            retStatus.status = BZ_BT_FAIL;
            break;
        case BZ_BT_DECOR_INVERT:
-            if (inStatus == BZ_BT_FAIL)
-                status = BZ_BT_SUCCESS;
-            if (inStatus == BZ_BT_SUCCESS)
-                status = BZ_BT_FAIL;
+            if (inStatus.status == BZ_BT_FAIL)
+                retStatus.status = BZ_BT_SUCCESS;
+            if (inStatus.status == BZ_BT_SUCCESS)
+                retStatus.status = BZ_BT_FAIL;
            break;
        case BZ_BT_DECOR_UNTIL_SUCCESS:
-            if (inStatus == BZ_BT_SUCCESS)
-                status = BZ_BT_SUCCESS;
+            if (inStatus.status == BZ_BT_SUCCESS)
+                retStatus.status = BZ_BT_SUCCESS;
            else
-                status = BZ_BT_RUNNING;
+                retStatus.status = BZ_BT_RUNNING;
            break;
        case BZ_BT_DECOR_UNTIL_FAIL:
-            if (inStatus == BZ_BT_FAIL)
-                status = BZ_BT_SUCCESS;
+            if (inStatus.status == BZ_BT_FAIL)
+                retStatus.status = BZ_BT_SUCCESS;
            else
-                status = BZ_BT_RUNNING;
+                retStatus.status = BZ_BT_RUNNING;
            break;
        case BZ_BT_DECOR_REPEAT:
-            BZ_ASSERT(nodeState->node == node);
+            BZ_ASSERT(nodeMatchesState(node, &retStatus.state));
+            BZ_ASSERT(nodeState);
            nodeState->as.repeat.iter++;
            if (nodeState->as.repeat.iter >= node->as.repeat.n) {
-                bzBTStateRelease(newState, nodeState);
-                status = inStatus;
+                BZ_ASSERT(nodeState == execStatePopFront(&retStatus.state));
+                releaseNodeState(statePool, nodeState);
+                retStatus.status = inStatus.status;
                break;
            }
-            status = BZ_BT_RUNNING;
+            retStatus.status = BZ_BT_RUNNING;
            break;
        default:
            break;
    }
-    return status;
+    return retStatus;
 }
-static inline BzBTStatus bzBTExecuteNode(const BzBTNode *node, f32 dt,
-                                         BzBTNodeState *nodeState,
-                                         BzBTState *oldState, BzBTState *newState) {
-    BzBTStatus status = BZ_BT_ERROR;
+static inline BzBTExecStatus bzBTExecuteNode(const BzBTNode *node, f32 dt,
+                                             BzBTExecState *nodeState, BzObjectPool *statePool) {
+    BzBTExecStatus status = { .status = BZ_BT_ERROR };
    switch (node->type) {
        case BZ_BT_COMP_SELECTOR:
        case BZ_BT_COMP_SEQUENCE:
        case BZ_BT_COMP_PARALLEL_SELECTOR:
        case BZ_BT_COMP_PARALLEL_SEQUENCE:
-            status = bzBTExecuteComposite(node, dt, nodeState, oldState, newState);
+            //status = bzBTExecuteComposite(node, dt, nodeState, statePool);
            break;
        case BZ_BT_DECOR_DUMMY:
        case BZ_BT_DECOR_SUCCESS:
@@ -505,11 +554,12 @@ static inline BzBTStatus bzBTExecuteNode(const BzBTNode *node, f32 dt,
        case BZ_BT_DECOR_UNTIL_FAIL:
        case BZ_BT_DECOR_REPEAT:
        case BZ_BT_DECOR_DELAY:
-            status = bzBTExecuteDecorator(node, dt, nodeState, oldState, newState);
+            status = bzBTExecuteDecorator(node, dt, nodeState, statePool);
            break;
        case BZ_BT_ACTION:
            BZ_ASSERT(node->as.action.fn);
-            return node->as.action.fn(oldState->userData, dt);
+            status.status = node->as.action.fn(nodeState->userData, dt);
+            break;
    }
    return status;
 }
@@ -522,27 +572,22 @@ BzBTStatus bzBTExecute(BzBTState *state, f32 dt) {
        return BZ_BT_FAIL;
    }

-    BzBTState newState = {
-            ._first = NULL,
-            ._last = NULL,
-            .nodeStatePool = state->nodeStatePool
-    };
-
    BzBTNodeState *first = state->_first;
    const BzBTNode *firstNode = first ? first->node : state->root;
-    BzBTStatus status = bzBTExecuteNode(firstNode, dt, first, state, &newState);
+    BzBTExecState execState = {
+            .first = state->_first,
+            .last = state->_last,
+            .userData = state->userData
+    };
+    BzBTExecStatus status = bzBTExecuteNode(firstNode, dt, &execState, state->nodeStatePool);
+    if (status.status == BZ_BT_ERROR)
+        execStateClear(&status.state, state->nodeStatePool);
+    BZ_ASSERT(execState.first == NULL && execState.last == NULL);

-    // Release leftover states
-    BzBTNodeState *pState = state->_first;
-    while (pState) {
-        BzBTNodeState *next = pState->next;
-        bzBTStateRelease(state, pState);
-        pState = next;
-    }
-    state->_first = newState._first;
-    state->_last = newState._last;
+    state->_first = status.state.first;
+    state->_last = status.state.last;

-    switch (status) {
+    switch (status.status) {
        case BZ_BT_SUCCESS:
            state->onSuccess && state->onSuccess(state->userData, dt);
            break;
@@ -556,5 +601,5 @@ BzBTStatus bzBTExecute(BzBTState *state, f32 dt) {
            break;
    }

-    return status;
+    return status.status;
 }
--- a/engine/tests/btree_test.c
+++ b/engine/tests/btree_test.c
@@ -117,6 +117,8 @@ void igVisualizeBTState(const BzBTNode *node, const BzBTNodeState *state, bool s
 void igRenderBT(BzBTState *state) {
    const BzBTNode *root = state->root;
    if (igBegin("BehaviourTree", NULL, 0)) {
+        igText("NodeState pool: %d", bzObjectPoolGetNumFree(nodeStatePool));
+        igSeparatorText("");
        igVisualizeBTState(root, state->_first, false, 0);
    }
    igEnd();
@@ -126,6 +128,7 @@ void render(float dt, int *game) {
    ClearBackground(WHITE);

    BzBTStatus status = bzBTExecute(&agentState, dt);
+    assert(status != BZ_BT_ERROR);

    rlImGuiBegin();
    igRenderBT(&agentState);