Detour crowd experiments

src/world/Actor/BNpc.cpp

@@ -283,8 +283,11 @@ bool Sapphire::Entity::BNpc::moveTo( const FFXIVARR_POSITION3& pos )
     }
   }
 
-
   step();
+  auto pos1 = pNaviProvider->getMovePos( *this );
+
+  Logger::debug( "{} {} {}", pos1.x, pos1.y, pos1.z );
+  setPos( pos1 );
   m_pCurrentZone->updateActorPosition( *this );
   return false;
 }
@@ -471,6 +474,14 @@ void Sapphire::Entity::BNpc::update( uint64_t tickCount )
 
     case BNpcState::Roaming:
     {
+      auto pNaviMgr = m_pFw->get< World::Manager::NaviMgr >();
+      auto pNaviProvider = pNaviMgr->getNaviProvider( m_pCurrentZone->getBgPath() );
+
+      if( !pNaviProvider )
+      {
+        pNaviProvider->setMoveTarget( *this, m_roamPos );
+      }
+
       if( moveTo( m_roamPos ) )
       {
         m_lastRoamTargetReached = Util::getTimeSeconds();

src/world/Actor/Chara.cpp

@@ -702,3 +702,13 @@ void Sapphire::Entity::Chara::setDirectorId( uint32_t directorId )
 {
   m_directorId = directorId;
 }
+
+uint32_t Sapphire::Entity::Chara::getAgentId() const
+{
+  return m_agentId;
+}
+
+void Sapphire::Entity::Chara::setAgentId( uint32_t agentId )
+{
+  m_agentId = agentId;
+}

src/world/Actor/Chara.h

@@ -128,6 +128,9 @@ namespace Sapphire::Entity
     std::map< uint8_t, StatusEffect::StatusEffectPtr > m_statusEffectMap;
     FrameworkPtr m_pFw;
 
+    /*! Detour Crowd AgentId */
+    uint32_t m_agentId;
+
   public:
     Chara( Common::ObjKind type, FrameworkPtr pFw );
 
@@ -269,6 +272,9 @@ namespace Sapphire::Entity
     uint32_t getDirectorId() const;
     void setDirectorId( uint32_t directorId );
 
+    uint32_t getAgentId() const;
+    void setAgentId( uint32_t agentId );
+
   };
 
 }

src/world/CMakeLists.txt

@@ -33,7 +33,8 @@ set_target_properties( world
 target_link_libraries( world
                          PUBLIC
                            common 
-                            Detour)
+                            Detour
+                            DetourCrowd )
 target_include_directories( world
                               PUBLIC
                                 "${CMAKE_CURRENT_SOURCE_DIR}"

src/world/Manager/TerritoryMgr.cpp

@@ -180,9 +180,12 @@ bool Sapphire::World::Manager::TerritoryMgr::createDefaultTerritories()
 
     uint32_t guid = getNextInstanceId();
 
-    auto pNaviMgr = framework()->get< Manager::NaviMgr >();
+    auto pZone = make_Zone( territoryTypeId, guid, territoryInfo->name, pPlaceName->name, framework() );
+    pZone->init();
+
     std::string bgPath = territoryInfo->bg;
-    bool hasNaviMesh = pNaviMgr->setupTerritory( bgPath );
+
+    bool hasNaviMesh = pZone->getNaviProvider() != nullptr;
 
     Logger::info( "{0}\t{1}\t{2}\t{3:<10}\t{4}\t{5}\t{6}",
                   territoryTypeId,
@@ -193,9 +196,6 @@ bool Sapphire::World::Manager::TerritoryMgr::createDefaultTerritories()
                   hasNaviMesh ? "NAVI" : "",
                   pPlaceName->name );
 
-    auto pZone = make_Zone( territoryTypeId, guid, territoryInfo->name, pPlaceName->name, framework() );
-    pZone->init();
-
     InstanceIdToZonePtrMap instanceMap;
     instanceMap[ guid ] = pZone;
     m_guIdToZonePtrMap[ guid ] = pZone;

src/world/Navi/NaviProvider.cpp

@@ -4,6 +4,9 @@
 #include <Logging/Logger.h>
 #include <ServerMgr.h>
 
+#include "Actor/Actor.h"
+#include "Actor/Chara.h"
+
 #include <Manager/RNGMgr.h>
 
 #include "NaviProvider.h"
@@ -40,6 +43,11 @@ bool Sapphire::World::Navi::NaviProvider::init()
     if( !loadMesh( baseMesh.string() ) )
       return false;
 
+    m_pCrowd = std::make_unique< dtCrowd >();
+
+    if( !m_pCrowd->init( 1000, 10.f, m_naviMesh ) )
+      return false;
+
     initQuery();
 
     return true;
@@ -521,3 +529,65 @@ bool Sapphire::World::Navi::NaviProvider::loadMesh( const std::string& path )
 
   return true;
 }
+
+int32_t Sapphire::World::Navi::NaviProvider::addAgent( Entity::Chara& chara )
+{
+  dtCrowdAgentParams params;
+  std::memset( &params, 0, sizeof( params ) );
+  params.height = 7.f;
+  params.maxAcceleration = 8.f;
+  params.maxSpeed = 3.5f;
+  params.radius = 5.f;
+  params.collisionQueryRange = params.radius * 12.0f;
+  params.pathOptimizationRange = params.radius * 30.0f;
+
+  float position[] = { chara.getPos().x, chara.getPos().y, chara.getPos().z };
+  return m_pCrowd->addAgent( position, &params );
+}
+
+void Sapphire::World::Navi::NaviProvider::updateCrowd( float timeInSeconds )
+{
+  dtCrowdAgentDebugInfo info;
+  m_pCrowd->update( timeInSeconds, &info );
+}
+
+void Sapphire::World::Navi::NaviProvider::removeAgent( Sapphire::Entity::Chara& chara )
+{
+  m_pCrowd->removeAgent( chara.getAgentId() );
+}
+
+void Sapphire::World::Navi::NaviProvider::calcVel( float* vel, const float* pos, const float* tgt, const float speed )
+{
+  dtVsub( vel, tgt, pos );
+  vel[ 1 ] = 0.0;
+  dtVnormalize( vel );
+  dtVscale( vel, vel, speed );
+}
+
+void Sapphire::World::Navi::NaviProvider::setMoveTarget( Entity::Chara& chara,
+                                                         const Sapphire::Common::FFXIVARR_POSITION3& endPos )
+{
+  // Find nearest point on navmesh and set move request to that location.
+  dtNavMeshQuery* navquery = m_naviMeshQuery;
+
+  const dtQueryFilter* filter = m_pCrowd->getFilter( 0 );
+  const float* halfExtents = m_pCrowd->getQueryExtents();
+
+  float vel[ 3 ];
+  float p[ 3 ] = { chara.getPos().x, chara.getPos().y, chara.getPos().z };
+
+  const dtCrowdAgent* ag = m_pCrowd->getAgent( chara.getAgentId() );
+  if( ag && ag->active )
+  {
+    calcVel( vel, ag->npos, p, ag->params.maxSpeed );
+    m_pCrowd->requestMoveVelocity( chara.getAgentId(), vel );
+  }
+}
+
+Sapphire::Common::FFXIVARR_POSITION3 Sapphire::World::Navi::NaviProvider::getMovePos( Entity::Chara& chara )
+{
+  const dtCrowdAgent* ag = m_pCrowd->getAgent( chara.getAgentId() );
+  if( !ag )
+    return { 0.f, 0.f, 0.f };
+  return { ag->npos[ 0 ], ag->npos[ 1 ], ag->npos[ 2 ] };
+}

src/world/Navi/NaviProvider.h

@@ -5,6 +5,7 @@
 #include "ForwardsZone.h"
 #include <recastnavigation/Detour/Include/DetourNavMesh.h>
 #include <recastnavigation/Detour/Include/DetourNavMeshQuery.h>
+#include <recastnavigation/DetourCrowd/Include/DetourCrowd.h>
 
 namespace Sapphire::World::Navi
 {
@@ -47,11 +48,24 @@ namespace Sapphire::World::Navi
 
     bool hasNaviMesh() const;
 
+    int32_t addAgent( Entity::Chara& chara );
+
+    void removeAgent( Entity::Chara& chara );
+
+    void updateCrowd( float timeInSeconds );
+
+    static void calcVel( float* vel, const float* pos, const float* tgt, const float speed );
+
+    void setMoveTarget( Entity::Chara& chara, const Common::FFXIVARR_POSITION3& endPos );
+
+    Common::FFXIVARR_POSITION3 getMovePos( Entity::Chara& chara );
+
   protected:
     std::string m_internalName;
 
     dtNavMesh* m_naviMesh;
     dtNavMeshQuery* m_naviMeshQuery;
+    std::unique_ptr< dtCrowd > m_pCrowd;
 
     float m_polyFindRange[ 3 ];
 
@@ -63,6 +77,8 @@ namespace Sapphire::World::Navi
 		                     const dtPolyRef* path, const int32_t pathSize, float* steerPos, uint8_t& steerPosFlag,
 			       	           dtPolyRef& steerPosRef, float* outPoints = 0, int32_t* outPointCount = 0 );
 
+
+
     FrameworkPtr m_pFw;
   };
 

src/world/Territory/Zone.cpp

@@ -19,6 +19,7 @@
 #include "InstanceContent.h"
 #include "QuestBattle.h"
 #include "Manager/TerritoryMgr.h"
+#include "Navi/Naviprovider.h"
 
 #include "Session.h"
 #include "Actor/Chara.h"
@@ -41,7 +42,8 @@
 #include "Zone.h"
 #include "Framework.h"
 
-#include <Manager/RNGMgr.h>
+#include "Manager/RNGMgr.h"
+#include "Manager/NaviMgr.h"
 
 using namespace Sapphire::Common;
 using namespace Sapphire::Network::Packets;
@@ -129,6 +131,11 @@ bool Sapphire::Zone::init()
     // all good
   }
 
+  auto pNaviMgr = m_pFw->get< World::Manager::NaviMgr >();
+  pNaviMgr->setupTerritory( m_territoryTypeInfo->bg );
+
+  m_pNaviProvider = pNaviMgr->getNaviProvider( m_territoryTypeInfo->bg );
+
   return true;
 }
 
@@ -231,10 +238,16 @@ void Sapphire::Zone::pushActor( Entity::ActorPtr pActor )
     }
   }
 
+  int32_t agentId = -1;
+
   if( pActor->isPlayer() )
   {
     auto pPlayer = pActor->getAsPlayer();
 
+    if( m_pNaviProvider )
+      agentId = m_pNaviProvider->addAgent( *pPlayer );
+    pPlayer->setAgentId( agentId );
+
     auto pServerZone = m_pFw->get< World::ServerMgr >();
     m_playerMap[ pPlayer->getId() ] = pPlayer;
     updateCellActivity( cx, cy, 2 );
@@ -243,6 +256,10 @@ void Sapphire::Zone::pushActor( Entity::ActorPtr pActor )
   {
     auto pBNpc = pActor->getAsBNpc();
 
+    if( m_pNaviProvider )
+      agentId = m_pNaviProvider->addAgent( *pBNpc );
+    pBNpc->setAgentId( agentId );
+
     m_bNpcMap[ pBNpc->getId() ] = pBNpc;
     updateCellActivity( cx, cy, 2 );
 
@@ -263,6 +280,9 @@ void Sapphire::Zone::removeActor( Entity::ActorPtr pActor )
   if( pActor->isPlayer() )
   {
 
+    if( m_pNaviProvider )
+      m_pNaviProvider->removeAgent( *pActor->getAsChara() );
+
     // If it's a player and he's inside boundaries - update his nearby cells
     if( pActor->getPos().x <= _maxX && pActor->getPos().x >= _minX &&
         pActor->getPos().z <= _maxY && pActor->getPos().z >= _minY )
@@ -278,6 +298,8 @@ void Sapphire::Zone::removeActor( Entity::ActorPtr pActor )
   }
   else if( pActor->isBattleNpc() )
   {
+    if( m_pNaviProvider )
+      m_pNaviProvider->removeAgent( *pActor->getAsChara() );
     m_bNpcMap.erase( pActor->getId() );
   }
 
@@ -451,6 +473,9 @@ bool Sapphire::Zone::update( uint64_t tickCount )
   //TODO: this should be moved to a updateWeather call and pulled out of updateSessions
   bool changedWeather = checkWeather();
 
+  if( m_pNaviProvider )
+    m_pNaviProvider->updateCrowd( 0.001f * tickCount  );
+
   updateSessions( tickCount, changedWeather );
   onUpdate( tickCount );
 
@@ -983,3 +1008,8 @@ Sapphire::Entity::BNpcPtr Sapphire::Zone::getActiveBNpcByLevelId( uint32_t level
   }
   return nullptr;
 }
+
+std::shared_ptr< Sapphire::World::Navi::NaviProvider > Sapphire::Zone::getNaviProvider()
+{
+  return m_pNaviProvider;
+}

src/world/Territory/Zone.h

@@ -63,6 +63,7 @@ namespace Sapphire
     std::vector< Entity::SpawnGroup > m_spawnGroups;
 
     uint32_t m_effectCounter;
+    std::shared_ptr< World::Navi::NaviProvider > m_pNaviProvider;
 
   public:
     Zone();
@@ -171,6 +172,8 @@ namespace Sapphire
     void updateSpawnPoints();
 
     uint32_t getNextEffectSequence();
+
+    std::shared_ptr< World::Navi::NaviProvider > getNaviProvider();
   };
 
 }