Add Move To node to BT

Flax.sln.DotSettings

@@ -321,6 +321,7 @@
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=raycast/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=raycasting/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=raycasts/@EntryIndexedValue">True</s:Boolean>
+	<s:Boolean x:Key="/Default/UserDictionary/Words/=reachability/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=readback/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=Reimports/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=reimported/@EntryIndexedValue">True</s:Boolean>

Source/Engine/AI/Behavior.cpp

@@ -4,6 +4,7 @@
 #include "BehaviorKnowledge.h"
 #include "BehaviorTreeNodes.h"
 #include "Engine/Engine/Time.h"
+#include "Engine/Profiler/ProfilerCPU.h"
 
 Behavior::Behavior(const SpawnParams& params)
     : Script(params)
@@ -15,6 +16,8 @@ Behavior::Behavior(const SpawnParams& params)
 
 void Behavior::StartLogic()
 {
+    PROFILE_CPU();
+
     // Ensure to have tree loaded on begin play
     CHECK(Tree && !Tree->WaitForLoaded());
     BehaviorTree* tree = Tree.Get();
@@ -30,6 +33,7 @@ void Behavior::StopLogic(BehaviorUpdateResult result)
 {
     if (_result != BehaviorUpdateResult::Running || result == BehaviorUpdateResult::Running)
         return;
+    PROFILE_CPU();
     _accumulatedTime = 0.0f;
     _totalTime = 0;
     _result = result;
@@ -37,6 +41,7 @@ void Behavior::StopLogic(BehaviorUpdateResult result)
 
 void Behavior::ResetLogic()
 {
+    PROFILE_CPU();
     const bool isActive = _result == BehaviorUpdateResult::Running;
     if (isActive)
         StopLogic();
@@ -61,6 +66,7 @@ void Behavior::OnLateUpdate()
 {
     if (_result != BehaviorUpdateResult::Running)
         return;
+    PROFILE_CPU();
     const BehaviorTree* tree = Tree.Get();
     if (!tree || !tree->Graph.Root)
     {

Source/Engine/AI/BehaviorTreeNodes.cpp

@@ -8,7 +8,14 @@
 #if USE_CSHARP
 #include "Engine/Scripting/ManagedCLR/MClass.h"
 #endif
+#include "Engine/Engine/Engine.h"
+#include "Engine/Engine/Time.h"
 #include "Engine/Level/Actor.h"
+#include "Engine/Navigation/NavMeshRuntime.h"
+#include "Engine/Physics/Actors/RigidBody.h"
+#include "Engine/Physics/Colliders/CapsuleCollider.h"
+#include "Engine/Physics/Colliders/CharacterController.h"
+#include "Engine/Profiler/ProfilerCPU.h"
 #include "Engine/Serialization/Serialization.h"
 
 bool IsAssignableFrom(const StringAnsiView& to, const StringAnsiView& from)
@@ -329,6 +336,202 @@ BehaviorUpdateResult BehaviorTreeForceFinishNode::Update(const BehaviorUpdateCon
     return Result;
 }
 
+bool BehaviorTreeMoveToNode::Move(Actor* agent, const Vector3& move) const
+{
+    agent->AddMovement(move);
+    return false;
+}
+
+NavMeshRuntime* BehaviorTreeMoveToNode::GetNavMesh(Actor* agent) const
+{
+    return NavMeshRuntime::Get();
+}
+
+void BehaviorTreeMoveToNode::GetAgentSize(Actor* agent, float& outRadius, float& outHeight) const
+{
+    if (const auto* characterController = Cast<CharacterController>(agent))
+    {
+        // Character Controller is an capsule
+        outRadius = characterController->GetRadius();
+        outHeight = characterController->GetHeight() + 2 * outRadius;
+        return;
+    }
+    if (const auto* rigidBody = Cast<RigidBody>(agent))
+    {
+        // Rigid Body with a single Capsule collider (directed Up)
+        Array<Collider*, InlinedAllocation<16>> colliders;
+        rigidBody->GetColliders(colliders);
+        const auto* capsuleCollider = colliders.Count() == 1 ? (CapsuleCollider*)colliders[0] : nullptr;
+        if (capsuleCollider && (capsuleCollider->GetLocalOrientation() == Quaternion::Euler(0, 0, 90) || capsuleCollider->GetLocalOrientation() == Quaternion::Euler(0, 0, -90)))
+        {
+            outRadius = capsuleCollider->GetRadius();
+            outHeight = capsuleCollider->GetHeight() + 2 * outRadius;
+            return;
+        }
+    }
+
+    // Estimate actor bounds to extract capsule information
+    const BoundingBox box = agent->GetBox();
+    const BoundingSphere sphere = agent->GetSphere();
+    outRadius = sphere.Radius;
+    outHeight = box.GetSize().Y;
+}
+
+int32 BehaviorTreeMoveToNode::GetStateSize() const
+{
+    return sizeof(State);
+}
+
+void BehaviorTreeMoveToNode::InitState(const BehaviorUpdateContext& context)
+{
+    auto state = GetState<State>(context.Memory);
+    new(state)State();
+    state->Node = this;
+    state->Knowledge = context.Knowledge;
+
+    // Get agent to move
+    if (Agent.Path.HasChars())
+        state->Agent = Agent.Get(context.Knowledge);
+    else
+        state->Agent = context.Behavior->GetActor();
+}
+
+void BehaviorTreeMoveToNode::ReleaseState(const BehaviorUpdateContext& context)
+{
+    auto state = GetState<State>(context.Memory);
+    if (state->HasTick)
+        Engine::Update.Unbind<State, &State::OnUpdate>(state);
+    state->~State();
+}
+
+BehaviorUpdateResult BehaviorTreeMoveToNode::Update(const BehaviorUpdateContext& context)
+{
+    auto state = GetState<State>(context.Memory);
+    if (state->Agent == nullptr)
+        return BehaviorUpdateResult::Failed;
+    bool repath = !state->HasPath;
+
+    Vector3 goalLocation = state->GoalLocation;
+    if (repath || UseTargetGoalUpdate)
+    {
+        // Get current goal location
+        const Actor* target = Target.Get(context.Knowledge);
+        if (target)
+            goalLocation = target->GetPosition();
+        else
+            goalLocation = TargetLocation.Get(context.Knowledge);
+        repath |= Vector3::Distance(goalLocation, state->GoalLocation) > TargetGoalUpdateTolerance;
+        state->GoalLocation = goalLocation;
+    }
+
+    if (repath)
+    {
+        // Clear path
+        state->HasPath = false;
+        state->Path.Clear();
+        state->AgentOffset = Vector3::Zero;
+        state->UpVector = Float3::Up;
+        state->NavAgentRadius = 0;
+
+        // Find a new path
+        const Vector3 agentLocation = state->Agent->GetPosition();
+        if (UsePathfinding)
+        {
+            const NavMeshRuntime* navMesh = GetNavMesh(state->Agent);
+            if (!navMesh)
+                return BehaviorUpdateResult::Failed;
+            NavMeshPathFlags pathFlags;
+            if (!navMesh->FindPath(agentLocation, goalLocation, state->Path, pathFlags))
+                return BehaviorUpdateResult::Failed;
+            if (!UsePartialPath && EnumHasAnyFlags(pathFlags, NavMeshPathFlags::PartialPath))
+                return BehaviorUpdateResult::Failed;
+            state->UpVector = Float3::Transform(Float3::Up, navMesh->Properties.Rotation);
+            state->NavAgentRadius = navMesh->Properties.Agent.Radius;
+
+            // Place start and end on navmesh
+            navMesh->ProjectPoint(state->Path.First(), state->Path.First());
+            navMesh->ProjectPoint(state->Path.Last(), state->Path.Last());
+
+            // Calculate offset between path and the agent (aka feet offset)
+            state->AgentOffset = state->Path.First() - agentLocation;
+        }
+        else
+        {
+            // Dummy movement
+            state->Path.Resize(2);
+            state->Path.Get()[0] = agentLocation;
+            state->Path.Get()[1] = goalLocation;
+        }
+
+        // Start path following
+        state->HasPath = true;
+        state->TargetPathIndex = 1;
+        state->Result = BehaviorUpdateResult::Running;
+
+        // TODO: add path debugging in Editor (eg. via BT window)
+
+        // Register for ticking the path following logic at game update rate (BT usually use lower FPS due to performance)
+        if (!state->HasTick)
+        {
+            state->HasTick = true;
+            Engine::Update.Bind<State, &State::OnUpdate>(state);
+        }
+    }
+
+    return state->Result;
+}
+
+void BehaviorTreeMoveToNode::State::OnUpdate()
+{
+    if (Result != BehaviorUpdateResult::Running)
+        return;
+    PROFILE_CPU();
+
+    // Get agent properties
+    const Vector3 agentLocation = Agent->GetPosition();
+    float movementSpeed;
+    if (!Node->MovementSpeed.TryGet(Knowledge, movementSpeed))
+        movementSpeed = 100;
+    float agentRadius = 30.0f, agentHeight = 100.0f;
+    Node->GetAgentSize(Agent, agentRadius, agentHeight);
+
+    // Test if agent reached the next path segment
+    Vector3 pathSegmentEnd = Path[TargetPathIndex];
+    const Vector3 agentLocationOnPath = agentLocation + AgentOffset;
+    const bool isLastSegment = TargetPathIndex + 1 == Path.Count();
+    float testRadius;
+    if (isLastSegment)
+        testRadius = agentRadius + Node->AcceptableRadius;
+    else
+        testRadius = agentRadius * 0.05f + Math::Max(agentRadius - NavAgentRadius, 0.0f); // 5% threshold of agent radius and diff between navmesh vs agent radius as threshold for path segments reaching
+    const float acceptableHeightPercentage = 1.05f;
+    const float testHeight = agentHeight * acceptableHeightPercentage;
+    const Vector3 toGoal = agentLocationOnPath - pathSegmentEnd;
+    const float toGoalHeightDiff = (toGoal * UpVector).SumValues();
+    if (toGoal.Length() <= testRadius && toGoalHeightDiff <= testHeight)
+    {
+        TargetPathIndex++;
+        if (TargetPathIndex == Path.Count())
+        {
+            // Goal reached!
+            Result = BehaviorUpdateResult::Success;
+            return;
+        }
+        pathSegmentEnd = Path[TargetPathIndex];
+    }
+
+    // Move agent
+    const float maxMove = movementSpeed * Time::Update.DeltaTime.GetTotalSeconds();
+    if (maxMove <= ZeroTolerance)
+        return;
+    const Vector3 move = Vector3::MoveTowards(agentLocationOnPath, pathSegmentEnd, maxMove) - agentLocationOnPath;
+    if (Node->Move(Agent, move))
+    {
+        // Move failed!
+        Result = BehaviorUpdateResult::Failed;
+    }
+}
+
 void BehaviorTreeInvertDecorator::PostUpdate(const BehaviorUpdateContext& context, BehaviorUpdateResult& result)
 {
     if (result == BehaviorUpdateResult::Success)

Source/Engine/AI/BehaviorTreeNodes.h

@@ -7,6 +7,8 @@
 #include "BehaviorKnowledgeSelector.h"
 #include "Engine/Core/Collections/Array.h"
 #include "Engine/Core/Collections/BitArray.h"
+#include "Engine/Core/Math/Vector3.h"
+#include "Engine/Scripting/ScriptingObjectReference.h"
 #include "Engine/Content/AssetReference.h"
 #include "Engine/Level/Tags.h"
 
@@ -147,6 +149,7 @@ public:
     void ReleaseState(const BehaviorUpdateContext& context) override;
     BehaviorUpdateResult Update(const BehaviorUpdateContext& context) override;
 
+private:
     struct State
     {
         Array<byte> Memory;
@@ -170,6 +173,87 @@ public:
     BehaviorUpdateResult Update(const BehaviorUpdateContext& context) override;
 };
 
+/// <summary>
+/// Moves an actor to the specific target location. Uses pathfinding on navmesh.
+/// </summary>
+API_CLASS() class FLAXENGINE_API BehaviorTreeMoveToNode : public BehaviorTreeNode
+{
+    DECLARE_SCRIPTING_TYPE_WITH_CONSTRUCTOR_IMPL(BehaviorTreeMoveToNode, BehaviorTreeNode);
+    API_AUTO_SERIALIZATION();
+
+    // The agent actor to move. If not set, uses Behavior's parent actor.
+    API_FIELD(Attributes="EditorOrder(10)")
+    BehaviorKnowledgeSelector<Actor*> Agent;
+
+    // The agent movement speed. Default value is 100 units/second.
+    API_FIELD(Attributes="EditorOrder(15)")
+    BehaviorKnowledgeSelector<float> MovementSpeed;
+
+    // The target movement object.
+    API_FIELD(Attributes="EditorOrder(30)")
+    BehaviorKnowledgeSelector<Actor*> Target;
+
+    // The target movement location.
+    API_FIELD(Attributes="EditorOrder(35)")
+    BehaviorKnowledgeSelector<Vector3> TargetLocation;
+
+    // Threshold value between Agent and Target goal location for destination reach test.
+    API_FIELD(Attributes="EditorOrder(100), Limit(0)")
+    float AcceptableRadius = 5.0f;
+
+    // Threshold value for the Target actor location offset that will trigger re-pathing to find a new path.
+    API_FIELD(Attributes="EditorOrder(110), Limit(0)")
+    float TargetGoalUpdateTolerance = 4.0f;
+
+    // If checked, the movement will use navigation system pathfinding, otherwise direct motion to the target location will happen.
+    API_FIELD(Attributes="EditorOrder(120)")
+    bool UsePathfinding = true;
+
+    // If checked, the movement will start even if there is no direct path to the target (only partial).
+    API_FIELD(Attributes="EditorOrder(130)")
+    bool UsePartialPath = true;
+
+    // If checked, the target goal location will be updated while Target actor moves.
+    API_FIELD(Attributes="EditorOrder(140)")
+    bool UseTargetGoalUpdate = true;
+
+public:
+    // Applies the movement to the agent. Returns true if cannot move.
+    virtual bool Move(Actor* agent, const Vector3& move) const;
+
+    // Returns the navmesh to use for the path-finding. Can query nav agent properties from the agent actor to select navmesh.
+    virtual class NavMeshRuntime* GetNavMesh(Actor* agent) const;
+
+    // Returns the agent dimensions used for path following (eg. goal reachability test).
+    virtual void GetAgentSize(Actor* agent, float& outRadius, float& outHeight) const;
+
+public:
+    // [BehaviorTreeNode]
+    int32 GetStateSize() const override;
+    void InitState(const BehaviorUpdateContext& context) override;
+    void ReleaseState(const BehaviorUpdateContext& context) override;
+    BehaviorUpdateResult Update(const BehaviorUpdateContext& context) override;
+
+protected:
+    struct State
+    {
+        bool HasPath = false; // True if has Path computed
+        bool HasTick = false; // True if OnUpdate is binded
+        Vector3 GoalLocation;
+        BehaviorTreeMoveToNode* Node;
+        BehaviorKnowledge* Knowledge;
+        ScriptingObjectReference<Actor> Agent;
+        Array<Vector3> Path;
+        Vector3 AgentOffset; // Offset between agent position and path position (aka feet offset)
+        float NavAgentRadius; // Size of the agent used to compute navmesh
+        Float3 UpVector; // Path Up vector (from navmesh orientation)
+        int32 TargetPathIndex; // Index of the next path point to go to
+        BehaviorUpdateResult Result; // Current result of the OnUpdate
+
+        void OnUpdate();
+    };
+};
+
 /// <summary>
 /// Inverts node's result - fails if node succeeded or succeeds if node failed.
 /// </summary>