diff --git a/Source/Engine/Navigation/NavCrowd.cpp b/Source/Engine/Navigation/NavCrowd.cpp new file mode 100644 index 000000000..a41ead184 --- /dev/null +++ b/Source/Engine/Navigation/NavCrowd.cpp @@ -0,0 +1,118 @@ +// Copyright (c) 2012-2022 Wojciech Figat. All rights reserved. + +#include "NavCrowd.h" +#include "NavMesh.h" +#include "NavMeshRuntime.h" +#include "Engine/Profiler/ProfilerCPU.h" +#include + +NavCrowd::NavCrowd(const SpawnParams& params) + : ScriptingObject(params) +{ + _crowd = dtAllocCrowd(); +} + +NavCrowd::~NavCrowd() +{ + dtFreeCrowd(_crowd); +} + +bool NavCrowd::Init(float maxAgentRadius, int32 maxAgents, NavMesh* navMesh) +{ + NavMeshRuntime* navMeshRuntime = navMesh ? navMesh->GetRuntime() : NavMeshRuntime::Get(); + return Init(maxAgentRadius, maxAgents, navMeshRuntime); +} + +bool NavCrowd::Init(float maxAgentRadius, int32 maxAgents, NavMeshRuntime* navMesh) +{ + if (!_crowd || !navMesh) + return true; + return !_crowd->init(maxAgents, maxAgentRadius, navMesh->GetNavMesh()); +} + +int32 NavCrowd::AddAgent(const Vector3& position, const NavAgentProperties& properties) +{ + const Float3 pos = position; + dtCrowdAgentParams agentParams; + InitCrowdAgentParams(agentParams, properties); + return _crowd->addAgent(pos.Raw, &agentParams); +} + +Vector3 NavCrowd::GetAgentPosition(int32 id) const +{ + Vector3 result = Vector3::Zero; + const dtCrowdAgent* agent = _crowd->getAgent(id); + if (agent && agent->state != DT_CROWDAGENT_STATE_INVALID) + { + result = Float3(agent->npos); + } + return result; +} + +Vector3 NavCrowd::GetAgentVelocity(int32 id) const +{ + Vector3 result = Vector3::Zero; + const dtCrowdAgent* agent = _crowd->getAgent(id); + if (agent && agent->state != DT_CROWDAGENT_STATE_INVALID) + { + result = Float3(agent->vel); + } + return result; +} + +void NavCrowd::SetAgentProperties(int32 id, const NavAgentProperties& properties) +{ + dtCrowdAgentParams agentParams; + InitCrowdAgentParams(agentParams, properties); + _crowd->updateAgentParameters(id, &agentParams); +} + +void NavCrowd::SetAgentMoveTarget(int32 id, const Vector3& position) +{ + const float* extent = _crowd->getQueryExtents(); + const dtQueryFilter* filter = _crowd->getFilter(0); + const Float3 pointNavMesh = position; + dtPolyRef startPoly = 0; + Float3 nearestPt = pointNavMesh; + _crowd->getNavMeshQuery()->findNearestPoly(pointNavMesh.Raw, extent, filter, &startPoly, &nearestPt.X); + _crowd->requestMoveTarget(id, startPoly, nearestPt.Raw); +} + +void NavCrowd::SetAgentMoveVelocity(int32 id, const Vector3& velocity) +{ + const Float3 v = velocity; + _crowd->requestMoveVelocity(id, v.Raw); +} + +void NavCrowd::ResetAgentMove(int32 id) +{ + _crowd->resetMoveTarget(id); +} + +void NavCrowd::RemoveAgent(int32 id) +{ + _crowd->removeAgent(id); +} + +void NavCrowd::Update(float dt) +{ + PROFILE_CPU(); + _crowd->update(Math::Max(dt, ZeroTolerance), nullptr); +} + +void NavCrowd::InitCrowdAgentParams(dtCrowdAgentParams& agentParams, const NavAgentProperties& properties) +{ + agentParams.radius = properties.Radius; + agentParams.height = properties.Height; + agentParams.maxAcceleration = 10000.0f; + agentParams.maxSpeed = properties.MaxSpeed; + agentParams.collisionQueryRange = properties.Radius * 12.0f; + agentParams.pathOptimizationRange = properties.Radius * 30.0f; + agentParams.separationWeight = properties.CrowdSeparationWeight; + agentParams.updateFlags = DT_CROWD_ANTICIPATE_TURNS | DT_CROWD_OPTIMIZE_VIS | DT_CROWD_OPTIMIZE_TOPO | DT_CROWD_OBSTACLE_AVOIDANCE; + if (agentParams.separationWeight > 0.001f) + agentParams.updateFlags |= DT_CROWD_SEPARATION; + agentParams.obstacleAvoidanceType = 0; + agentParams.queryFilterType = 0; + agentParams.userData = this; +} diff --git a/Source/Engine/Navigation/NavCrowd.h b/Source/Engine/Navigation/NavCrowd.h new file mode 100644 index 000000000..e416bfb96 --- /dev/null +++ b/Source/Engine/Navigation/NavCrowd.h @@ -0,0 +1,105 @@ +// Copyright (c) 2012-2022 Wojciech Figat. All rights reserved. + +#pragma once + +#include "Engine/Scripting/ScriptingObject.h" +#include "NavigationTypes.h" + +class NavMesh; +class NavMeshRuntime; +class dtCrowd; +struct dtCrowdAgentParams; + +/// +/// Navigation steering behaviors system for a group of agents. Handles avoidance between agents by using an adaptive RVO sampling calculation. +/// +API_CLASS() class FLAXENGINE_API NavCrowd : public ScriptingObject +{ + DECLARE_SCRIPTING_TYPE(NavCrowd); +private: + dtCrowd* _crowd; + +public: + ~NavCrowd(); + + /// + /// Initializes the crowd. + /// + /// The maximum radius of any agent that will be added to the crowd. + /// maximum number of agents the crowd can manage. + /// The navigation mesh to use for crowd movement planning. Use null to pick the first navmesh. + /// True if failed, otherwise false. + API_FUNCTION() bool Init(float maxAgentRadius = 100.0f, int32 maxAgents = 25, NavMesh* navMesh = nullptr); + + /// + /// Initializes the crowd. + /// + /// The maximum radius of any agent that will be added to the crowd. + /// maximum number of agents the crowd can manage. + /// The navigation mesh to use for crowd movement planning. + /// True if failed, otherwise false. + bool Init(float maxAgentRadius, int32 maxAgents, NavMeshRuntime* navMesh); + + /// + /// Adds a new agent to the crowd. + /// + /// The agent position. + /// The agent properties. + /// The agent unique ID or -1 if failed to add it (eg. too many active agents). + API_FUNCTION() int32 AddAgent(const Vector3& position, const NavAgentProperties& properties); + + /// + /// Gets the agent current position. + /// + /// The agent ID. + /// The agent current position. + API_FUNCTION() Vector3 GetAgentPosition(int32 id) const; + + /// + /// Gets the agent current velocity (direction * speed). + /// + /// The agent ID. + /// The agent current velocity (direction * speed). + API_FUNCTION() Vector3 GetAgentVelocity(int32 id) const; + + /// + /// Updates the agent properties. + /// + /// The agent ID. + /// The agent properties. + API_FUNCTION() void SetAgentProperties(int32 id, const NavAgentProperties& properties); + + /// + /// Updates the agent movement target position. + /// + /// The agent ID. + /// The agent target position. + API_FUNCTION() void SetAgentMoveTarget(int32 id, const Vector3& position); + + /// + /// Updates the agent movement target velocity (direction * speed). + /// + /// The agent ID. + /// The agent target velocity (direction * speed). + API_FUNCTION() void SetAgentMoveVelocity(int32 id, const Vector3& velocity); + + /// + /// Resets any movement request for the specified agent. + /// + /// The agent ID. + API_FUNCTION() void ResetAgentMove(int32 id); + + /// + /// Removes the agent of the given ID. + /// + API_FUNCTION() void RemoveAgent(int32 id); + + /// + /// Updates the steering and positions of all agents. + /// + /// The simulation update delta time (in seconds). + API_FUNCTION() void Update(float dt); + +private: + void InitCrowdAgentParams(dtCrowdAgentParams& agentParams, const NavAgentProperties& properties); +}; diff --git a/Source/Engine/Navigation/NavMeshRuntime.h b/Source/Engine/Navigation/NavMeshRuntime.h index 98fc2e326..4c5c2b152 100644 --- a/Source/Engine/Navigation/NavMeshRuntime.h +++ b/Source/Engine/Navigation/NavMeshRuntime.h @@ -30,6 +30,9 @@ public: class FLAXENGINE_API NavMeshRuntime { public: + // Gets the first valid navigation mesh runtime. Return null if none created. + static NavMeshRuntime* Get(); + // Gets the navigation mesh runtime for a given navmesh name. Return null if missing. static NavMeshRuntime* Get(const StringView& navMeshName); diff --git a/Source/Engine/Navigation/Navigation.cpp b/Source/Engine/Navigation/Navigation.cpp index 1c3863374..248d7dd6d 100644 --- a/Source/Engine/Navigation/Navigation.cpp +++ b/Source/Engine/Navigation/Navigation.cpp @@ -25,6 +25,11 @@ namespace Array> NavMeshes; } +NavMeshRuntime* NavMeshRuntime::Get() +{ + return NavMeshes.Count() != 0 ? NavMeshes[0] : nullptr; +} + NavMeshRuntime* NavMeshRuntime::Get(const StringView& navMeshName) { NavMeshRuntime* result = nullptr; @@ -100,7 +105,7 @@ Color NavMeshRuntime::NavAreasColors[64]; bool NavAgentProperties::operator==(const NavAgentProperties& other) const { - return Math::NearEqual(Radius, other.Radius) && Math::NearEqual(Height, other.Height) && Math::NearEqual(StepHeight, other.StepHeight) && Math::NearEqual(MaxSlopeAngle, other.MaxSlopeAngle); + return Math::NearEqual(Radius, other.Radius) && Math::NearEqual(Height, other.Height) && Math::NearEqual(StepHeight, other.StepHeight) && Math::NearEqual(MaxSlopeAngle, other.MaxSlopeAngle) && Math::NearEqual(MaxSpeed, other.MaxSpeed) && Math::NearEqual(CrowdSeparationWeight, other.CrowdSeparationWeight); } bool NavAgentMask::IsAgentSupported(int32 agentIndex) const diff --git a/Source/Engine/Navigation/NavigationSettings.cs b/Source/Engine/Navigation/NavigationSettings.cs index 57b4d47ec..a13423cc5 100644 --- a/Source/Engine/Navigation/NavigationSettings.cs +++ b/Source/Engine/Navigation/NavigationSettings.cs @@ -23,6 +23,8 @@ namespace FlaxEditor.Content.Settings navMesh.Agent.Height = 144.0f; navMesh.Agent.StepHeight = 35.0f; navMesh.Agent.MaxSlopeAngle = 60.0f; + navMesh.Agent.MaxSpeed = 500.0f; + navMesh.Agent.CrowdSeparationWeight = 2.0f; navMesh.DefaultQueryExtent = new Vector3(50.0f, 250.0f, 50.0f); // Init nav areas @@ -53,6 +55,8 @@ namespace FlaxEditor.Content.Settings navMesh.Agent.Height = 144.0f; navMesh.Agent.StepHeight = 35.0f; navMesh.Agent.MaxSlopeAngle = 60.0f; + navMesh.Agent.MaxSpeed = 500.0f; + navMesh.Agent.CrowdSeparationWeight = 2.0f; navMesh.DefaultQueryExtent = new Vector3(50.0f, 250.0f, 50.0f); } @@ -130,7 +134,7 @@ namespace FlaxEngine /// public override string ToString() { - return $"Radius: {Radius}, Height: {Height}, StepHeight: {StepHeight}, MaxSlopeAngle: {MaxSlopeAngle}"; + return $"Radius: {Radius}, Height: {Height}, StepHeight: {StepHeight}, MaxSlopeAngle: {MaxSlopeAngle}, MaxSpeed: {MaxSpeed}"; } } } diff --git a/Source/Engine/Navigation/NavigationTypes.h b/Source/Engine/Navigation/NavigationTypes.h index 5cf8689ec..180b90f3f 100644 --- a/Source/Engine/Navigation/NavigationTypes.h +++ b/Source/Engine/Navigation/NavigationTypes.h @@ -43,6 +43,18 @@ API_STRUCT() struct FLAXENGINE_API NavAgentProperties : ISerializable API_FIELD(Attributes="EditorOrder(30)") float MaxSlopeAngle = 60.0f; + /// + /// The maximum movement speed (units/s). + /// + API_FIELD(Attributes="EditorOrder(40)") + float MaxSpeed = 500.0f; + + /// + /// The crowd agent separation weight that defines how aggressive the agent manager should be at avoiding collisions with this agent. + /// + API_FIELD(Attributes="EditorOrder(100)") + float CrowdSeparationWeight = 2.0f; + bool operator==(const NavAgentProperties& other) const; bool operator!=(const NavAgentProperties& other) const