HSM: Delay state transition till update is finished

bt/bt_state.cpp

@@ -63,13 +63,20 @@ void BTState::_setup() {
 }
 
 void BTState::_exit() {
+	if (tree_instance.is_valid()) {
+		tree_instance->abort();
+	} else {
+		ERR_PRINT_ONCE("BTState: BehaviorTree is not assigned.");
+	}
 	LimboState::_exit();
-	ERR_FAIL_NULL(tree_instance);
-	tree_instance->abort();
 }
 
 void BTState::_update(double p_delta) {
 	VCALL_ARGS(_update, p_delta);
+	if (!active) {
+		// Bail out if a transition happened in the meantime.
+		return;
+	}
 	ERR_FAIL_NULL(tree_instance);
 	int status = tree_instance->execute(p_delta);
 	if (status == BTTask::SUCCESS) {

hsm/limbo_hsm.cpp

@@ -88,7 +88,13 @@ void LimboHSM::_update(double p_delta) {
 }
 
 void LimboHSM::update(double p_delta) {
+	updating = true;
 	_update(p_delta);
+	updating = false;
+	if (next_active) {
+		_change_state(next_active);
+		next_active = nullptr;
+	}
 }
 
 void LimboHSM::add_transition(LimboState *p_from_state, LimboState *p_to_state, const StringName &p_event) {
@@ -170,7 +176,12 @@ bool LimboHSM::_dispatch(const StringName &p_event, const Variant &p_cargo) {
 				}
 			}
 			if (permitted) {
-				_change_state(to_state);
+				if (!updating) {
+					_change_state(to_state);
+				} else if (!next_active) {
+					// Only set next_active if we are not already in the process of changing states.
+					next_active = to_state;
+				}
 				event_consumed = true;
 			}
 		}
@@ -263,5 +274,6 @@ LimboHSM::LimboHSM() {
 	update_mode = UpdateMode::PHYSICS;
 	active_state = nullptr;
 	previous_active = nullptr;
+	next_active = nullptr;
 	initial_state = nullptr;
 }

hsm/limbo_hsm.h

@@ -29,7 +29,9 @@ class LimboHSM : public LimboState {
 	LimboState *initial_state;
 	LimboState *active_state;
 	LimboState *previous_active;
+	LimboState *next_active;
 	HashMap<uint64_t, LimboState *> transitions;
+	bool updating = false;
 
 	_FORCE_INLINE_ uint64_t _get_transition_key(LimboState *p_from_state, const StringName &p_event) {
 		uint64_t key = hash_djb2_one_64(Variant::OBJECT);