Add NavCrowd for navigation steering behaviors system for a group of agents

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 <ThirdParty/recastnavigation/DetourCrowd.h>
+
+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;
+}

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;
+
+/// <summary>
+/// Navigation steering behaviors system for a group of agents. Handles avoidance between agents by using an adaptive RVO sampling calculation.
+/// </summary>
+API_CLASS() class FLAXENGINE_API NavCrowd : public ScriptingObject
+{
+    DECLARE_SCRIPTING_TYPE(NavCrowd);
+private:
+    dtCrowd* _crowd;
+
+public:
+    ~NavCrowd();
+
+    /// <summary>
+    /// Initializes the crowd.
+    /// </summary>
+    /// <param name="maxAgentRadius">The maximum radius of any agent that will be added to the crowd.</param>
+    /// <param name="maxAgents"> maximum number of agents the crowd can manage.</param>
+    /// <param name="navMesh">The navigation mesh to use for crowd movement planning. Use null to pick the first navmesh.</param>
+    /// <returns>True if failed, otherwise false.</returns>
+    API_FUNCTION() bool Init(float maxAgentRadius = 100.0f, int32 maxAgents = 25, NavMesh* navMesh = nullptr);
+
+    /// <summary>
+    /// Initializes the crowd.
+    /// </summary>
+    /// <param name="maxAgentRadius">The maximum radius of any agent that will be added to the crowd.</param>
+    /// <param name="maxAgents"> maximum number of agents the crowd can manage.</param>
+    /// <param name="navMesh">The navigation mesh to use for crowd movement planning.</param>
+    /// <returns>True if failed, otherwise false.</returns>
+    bool Init(float maxAgentRadius, int32 maxAgents, NavMeshRuntime* navMesh);
+
+    /// <summary>
+    /// Adds a new agent to the crowd.
+    /// </summary>
+    /// <param name="position">The agent position.</param>
+    /// <param name="properties">The agent properties.</param>
+    /// <returns>The agent unique ID or -1 if failed to add it (eg. too many active agents).</returns>
+    API_FUNCTION() int32 AddAgent(const Vector3& position, const NavAgentProperties& properties);
+
+    /// <summary>
+    /// Gets the agent current position.
+    /// </summary>
+    /// <param name="id">The agent ID.</param>
+    /// <returns>The agent current position.</returns>
+    API_FUNCTION() Vector3 GetAgentPosition(int32 id) const;
+
+    /// <summary>
+    /// Gets the agent current velocity (direction * speed).
+    /// </summary>
+    /// <param name="id">The agent ID.</param>
+    /// <returns>The agent current velocity (direction * speed).</returns>
+    API_FUNCTION() Vector3 GetAgentVelocity(int32 id) const;
+
+    /// <summary>
+    /// Updates the agent properties.
+    /// </summary>
+    /// <param name="id">The agent ID.</param>
+    /// <param name="properties">The agent properties.</param>
+    API_FUNCTION() void SetAgentProperties(int32 id, const NavAgentProperties& properties);
+
+    /// <summary>
+    /// Updates the agent movement target position.
+    /// </summary>
+    /// <param name="id">The agent ID.</param>
+    /// <param name="position">The agent target position.</param>
+    API_FUNCTION() void SetAgentMoveTarget(int32 id, const Vector3& position);
+
+    /// <summary>
+    /// Updates the agent movement target velocity (direction * speed).
+    /// </summary>
+    /// <param name="id">The agent ID.</param>
+    /// <param name="velocity">The agent target velocity (direction * speed).</param>
+    API_FUNCTION() void SetAgentMoveVelocity(int32 id, const Vector3& velocity);
+
+    /// <summary>
+    /// Resets any movement request for the specified agent.
+    /// </summary>
+    /// <param name="id">The agent ID.</param>
+    API_FUNCTION() void ResetAgentMove(int32 id);
+
+    /// <summary>
+    /// Removes the agent of the given ID.
+    /// </summary>
+    API_FUNCTION() void RemoveAgent(int32 id);
+
+    /// <summary>
+    /// Updates the steering and positions of all agents.
+    /// </summary>
+    /// <param name="dt">The simulation update delta time (in seconds).</param>
+    API_FUNCTION() void Update(float dt);
+
+private:
+    void InitCrowdAgentParams(dtCrowdAgentParams& agentParams, const NavAgentProperties& properties);
+};

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);
 

Source/Engine/Navigation/Navigation.cpp

@@ -25,6 +25,11 @@ namespace
     Array<NavMeshRuntime*, InlinedAllocation<16>> 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

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
         /// <inheritdoc />
         public override string ToString()
         {
-            return $"Radius: {Radius}, Height: {Height}, StepHeight: {StepHeight}, MaxSlopeAngle: {MaxSlopeAngle}";
+            return $"Radius: {Radius}, Height: {Height}, StepHeight: {StepHeight}, MaxSlopeAngle: {MaxSlopeAngle}, MaxSpeed: {MaxSpeed}";
         }
     }
 }

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;
 
+    /// <summary>
+    /// The maximum movement speed (units/s).
+    /// </summary>
+    API_FIELD(Attributes="EditorOrder(40)")
+    float MaxSpeed = 500.0f;
+
+    /// <summary>
+    /// The crowd agent separation weight that defines how aggressive the agent manager should be at avoiding collisions with this agent.
+    /// </summary>
+    API_FIELD(Attributes="EditorOrder(100)")
+    float CrowdSeparationWeight = 2.0f;
+
     bool operator==(const NavAgentProperties& other) const;
 
     bool operator!=(const NavAgentProperties& other) const