diff --git a/Flax.sln.DotSettings b/Flax.sln.DotSettings index a910e0375..048a4c32a 100644 --- a/Flax.sln.DotSettings +++ b/Flax.sln.DotSettings @@ -321,6 +321,7 @@ True True True + True True True True diff --git a/Source/Engine/AI/Behavior.cpp b/Source/Engine/AI/Behavior.cpp index c992c721b..94f525541 100644 --- a/Source/Engine/AI/Behavior.cpp +++ b/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) { diff --git a/Source/Engine/AI/BehaviorTreeNodes.cpp b/Source/Engine/AI/BehaviorTreeNodes.cpp index 0ebf5e3c6..f53684bb6 100644 --- a/Source/Engine/AI/BehaviorTreeNodes.cpp +++ b/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(agent)) + { + // Character Controller is an capsule + outRadius = characterController->GetRadius(); + outHeight = characterController->GetHeight() + 2 * outRadius; + return; + } + if (const auto* rigidBody = Cast(agent)) + { + // Rigid Body with a single Capsule collider (directed Up) + Array> 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(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(context.Memory); + if (state->HasTick) + Engine::Update.Unbind(state); + state->~State(); +} + +BehaviorUpdateResult BehaviorTreeMoveToNode::Update(const BehaviorUpdateContext& context) +{ + auto state = GetState(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); + } + } + + 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) diff --git a/Source/Engine/AI/BehaviorTreeNodes.h b/Source/Engine/AI/BehaviorTreeNodes.h index a642b6911..6b1d4097e 100644 --- a/Source/Engine/AI/BehaviorTreeNodes.h +++ b/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 Memory; @@ -170,6 +173,87 @@ public: BehaviorUpdateResult Update(const BehaviorUpdateContext& context) override; }; +/// +/// Moves an actor to the specific target location. Uses pathfinding on navmesh. +/// +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 Agent; + + // The agent movement speed. Default value is 100 units/second. + API_FIELD(Attributes="EditorOrder(15)") + BehaviorKnowledgeSelector MovementSpeed; + + // The target movement object. + API_FIELD(Attributes="EditorOrder(30)") + BehaviorKnowledgeSelector Target; + + // The target movement location. + API_FIELD(Attributes="EditorOrder(35)") + BehaviorKnowledgeSelector 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 Agent; + Array 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(); + }; +}; + /// /// Inverts node's result - fails if node succeeded or succeeds if node failed. ///