Merge pull request #501 from goaaats/pathfinding

Basic pathfinding

src/world/Actor/BNpc.cpp

@@ -32,6 +32,9 @@
 #include "BNpcTemplate.h"
 #include "Manager/TerritoryMgr.h"
 #include "Common.h"
+#include "Framework.h"
+#include <Logging/Logger.h>
+#include <Manager/NaviMgr.h>
 
 using namespace Sapphire::Common;
 using namespace Sapphire::Network::Packets;
@@ -151,13 +154,85 @@ void Sapphire::Entity::BNpc::setState( BNpcState state )
   m_state = state;
 }
 
+void Sapphire::Entity::BNpc::step()
+{
+  if( m_naviLastPath.empty() )
+    // No path to track
+    return;
+
+  if( Util::distance( getPos().x, getPos().y, getPos().z, m_naviTarget.x, m_naviTarget.y, m_naviTarget.z ) <= 4 )
+  {
+    // Reached target
+    m_naviLastPath.clear();
+    return;
+  }
+
+  auto stepPos = m_naviLastPath[ m_naviPathStep ];
+
+  if( Util::distance( getPos().x, getPos().y, getPos().z, stepPos.x, stepPos.y, stepPos.z ) <= 4 && m_naviPathStep < m_naviLastPath.size() - 1 )
+  {
+    // Reached step in path
+    m_naviPathStep++;
+
+    stepPos = m_naviLastPath[ m_naviPathStep ];
+  }
+
+  // This is probably not a good way to do it but works fine for now
+  float rot = Util::calcAngFrom( getPos().x, getPos().z, stepPos.x, stepPos.z );
+  float newRot = PI - rot + ( PI / 2 );
+
+  face( stepPos );
+  float angle = Util::calcAngFrom( getPos().x, getPos().z, stepPos.x, stepPos.z ) + PI;
+
+  auto x = ( cosf( angle ) * 1.1f );
+  auto y = ( getPos().y + stepPos.y ) * 0.5f; // Get speed from somewhere else?
+  auto z = ( sinf( angle ) * 1.1f );
+
+  Common::FFXIVARR_POSITION3 newPos{ getPos().x + x, y, getPos().z + z };
+  setPos( newPos );
+  setRot( newRot );
+
+  sendPositionUpdate();
+}
+
 bool Sapphire::Entity::BNpc::moveTo( const FFXIVARR_POSITION3& pos )
 {
   if( Util::distance( getPos().x, getPos().y, getPos().z, pos.x, pos.y, pos.z ) <= 4 )
-    // reached destination
+    // Reached destination
     return true;
 
-  float rot = Util::calcAngFrom( getPos().x, getPos().z, pos.x, pos.z );
+  if( m_naviTarget.x == pos.x && m_naviTarget.y == pos.y && m_naviTarget.z == pos.z )
+    // Targets are the same
+    return false;
+
+  // Check if we have to recalculate
+  if( Util::getTimeMs() - m_naviLastUpdate > 500 )
+  {
+    auto pNaviMgr = m_pFw->get< World::Manager::NaviMgr >();
+    auto pNaviProvider = pNaviMgr->getNaviProvider( m_pCurrentZone->getInternalName() );
+
+    if( !pNaviProvider )
+    {
+      Logger::error( "No NaviProvider for zone#{0} - {1}", m_pCurrentZone->getGuId(), m_pCurrentZone->getInternalName() );
+      return false;
+    }
+
+    auto path = pNaviProvider->findFollowPath( m_pos, pos );
+
+    if( !path.empty() )
+    {
+      m_naviLastPath = path;
+      m_naviTarget = pos;
+      m_naviPathStep = 0;
+      m_naviLastUpdate = Util::getTimeMs();
+    }
+    else
+    {
+      Logger::debug( "No path found from x{0} y{1} z{2} to x{3} y{4} z{5} in {6}", getPos().x, getPos().y, getPos().z, pos.x, pos.y, pos.z, m_pCurrentZone->getInternalName() );
+    }
+  }
+  /*
+  float rot = Util::calcAngFrom( getPos().x, getPos().z, pos.x, pos.z ); 
   float newRot = PI - rot + ( PI / 2 );
 
   face( pos );
@@ -175,6 +250,7 @@ bool Sapphire::Entity::BNpc::moveTo( const FFXIVARR_POSITION3& pos )
   setRot( newRot );
 
   sendPositionUpdate();
+  */
 
   return false;
 }
@@ -304,7 +380,7 @@ void Sapphire::Entity::BNpc::update( int64_t currTime )
 {
   const uint8_t minActorDistance = 4;
   const uint8_t aggroRange = 8;
-  const uint8_t maxDistanceToOrigin = 30;
+  const uint8_t maxDistanceToOrigin = 40;
 
   switch( m_state )
   {
@@ -392,6 +468,8 @@ void Sapphire::Entity::BNpc::update( int64_t currTime )
       }
     }
   }
+
+  step();
 }
 
 void Sapphire::Entity::BNpc::onActionHostile( Sapphire::Entity::CharaPtr pSource )

src/world/Actor/BNpc.h

@@ -62,6 +62,9 @@ namespace Sapphire::Entity
     // return true if it reached the position
     bool moveTo( const Common::FFXIVARR_POSITION3& pos );
 
+    // processes movement
+    void step();
+
     void sendPositionUpdate();
 
     BNpcState getState() const;
@@ -106,6 +109,11 @@ namespace Sapphire::Entity
     BNpcState m_state;
     std::set< std::shared_ptr< HateListEntry > > m_hateList;
 
+    uint64_t m_naviLastUpdate;
+    std::vector< Common::FFXIVARR_POSITION3 > m_naviLastPath;
+    uint8_t m_naviPathStep;
+    Common::FFXIVARR_POSITION3 m_naviTarget;
+
   };
 
 }

src/world/CMakeLists.txt

@@ -19,7 +19,8 @@ file( GLOB SERVER_SOURCE_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
         Script/*.c*
         StatusEffect/*.c*
         Territory/*.c*
-        Territory/Housing/*.c*)
+        Territory/Housing/*.c*
+        Navi/*.c*)
 
 add_executable( world ${SERVER_SOURCE_FILES} )
 
@@ -30,10 +31,12 @@ set_target_properties( world
 
 target_link_libraries( world
                          PUBLIC
-                           common )
+                           common 
+                            Detour)
 target_include_directories( world
                               PUBLIC
-                                "${CMAKE_CURRENT_SOURCE_DIR}" )
+                                "${CMAKE_CURRENT_SOURCE_DIR}"
+                                    Detour )
 
 
 if( UNIX )

src/world/Manager/DebugCommandMgr.cpp

@@ -367,6 +367,18 @@ void Sapphire::World::Manager::DebugCommandMgr::set( char* data, Entity::Player&
       }
     }
   }
+  else if( subCommand == "mobaggro" )
+  {
+    auto inRange = player.getInRangeActors();
+
+    for( auto actor : inRange )
+    {
+      if( actor->getId() == player.getTargetId() && actor->getAsChara()->isAlive() )
+      {
+        actor->getAsBNpc()->onActionHostile( player.getAsChara() );
+      }
+    }
+  }
   else
   {
     player.sendUrgent( "{0} is not a valid SET command.", subCommand );

src/world/Manager/NaviMgr.cpp

@@ -0,0 +1,29 @@
+#include "NaviMgr.h"
+#include <Logging/Logger.h>
+
+Sapphire::World::Manager::NaviMgr::NaviMgr( FrameworkPtr pFw ) :
+  BaseManager( pFw ),
+  m_pFw( pFw )
+{
+}
+
+bool Sapphire::World::Manager::NaviMgr::setupTerritory( std::string internalName )
+{
+  auto provider = new NaviProvider( internalName );
+
+  if( provider->init() )
+  {
+    m_naviProviderTerritoryMap.insert( std::make_pair( internalName, provider ) );
+    return true;
+  }
+
+  return false;
+}
+
+Sapphire::NaviProvider* Sapphire::World::Manager::NaviMgr::getNaviProvider( std::string internalName )
+{
+  if( m_naviProviderTerritoryMap.find( internalName ) != m_naviProviderTerritoryMap.end() )
+    return m_naviProviderTerritoryMap[ internalName ];
+
+  return nullptr;
+}

src/world/Manager/NaviMgr.h

@@ -0,0 +1,31 @@
+#ifndef SAPPHIRE_NAVIMGR_H
+#define SAPPHIRE_NAVIMGR_H
+
+#include "Forwards.h"
+#include "BaseManager.h"
+
+#include <array>
+#include <Navi/NaviProvider.h>
+
+namespace Sapphire::World::Manager
+{
+  class NaviMgr : public BaseManager
+  {
+
+  public:
+
+    NaviMgr( FrameworkPtr pFw );
+    virtual ~NaviMgr() = default;
+
+    bool setupTerritory( std::string internalName );
+    NaviProvider* getNaviProvider( std::string internalName );
+
+  private:
+    FrameworkPtr m_pFw;
+
+    std::unordered_map<std::string, NaviProvider*> m_naviProviderTerritoryMap;
+  };
+
+}
+
+#endif // SAPPHIRE_NAVIMGR_H

src/world/Manager/TerritoryMgr.cpp

@@ -16,6 +16,7 @@
 #include "Territory/Land.h"
 #include "Territory/House.h"
 #include "Territory/Housing/HousingInteriorTerritory.h"
+#include "NaviMgr.h"
 
 Sapphire::World::Manager::TerritoryMgr::TerritoryMgr( Sapphire::FrameworkPtr pFw ) :
   BaseManager( pFw ),
@@ -163,13 +164,17 @@ bool Sapphire::World::Manager::TerritoryMgr::createDefaultTerritories()
 
     uint32_t guid = getNextInstanceId();
 
-    Logger::info( "{0}\t{1}\t{2}\t{3:<10}\t{4}\t{5}",
+    auto pNaviMgr = framework()->get< Manager::NaviMgr >();
+    bool hasNaviMesh = pNaviMgr->setupTerritory( territoryInfo->name );
+
+    Logger::info( "{0}\t{1}\t{2}\t{3:<10}\t{4}\t{5}\t{6}",
                   territoryTypeId,
                   guid,
                   territoryInfo->territoryIntendedUse,
                   territoryInfo->name,
                   ( isPrivateTerritory( territoryTypeId ) ? "PRIVATE" : "PUBLIC" ),
-                  pPlaceName->name );
+                  pPlaceName->name,
+                  hasNaviMesh ? "NAVI" : "");
 
     auto pZone = make_Zone( territoryTypeId, guid, territoryInfo->name, pPlaceName->name, framework() );
     pZone->init();

src/world/Navi/NaviProvider.cpp

@@ -0,0 +1,445 @@
+#include <Common.h>
+#include <Framework.h>
+#include <Territory/Zone.h>
+#include <Logging/Logger.h>
+
+#include "NaviProvider.h"
+
+#include <recastnavigation/Detour/Include/DetourNavMesh.h>
+#include <recastnavigation/Detour/Include/DetourNavMeshQuery.h>
+#include <DetourCommon.h>
+#include <recastnavigation/Recast/Include/Recast.h>
+#include <experimental/filesystem>
+
+
+Sapphire::NaviProvider::NaviProvider( std::string internalName ) :
+  m_naviMesh( nullptr ),
+  m_naviMeshQuery( nullptr ),
+  m_internalName( internalName )
+{
+  // Set defaults
+  m_polyFindRange[0] = 10;
+  m_polyFindRange[1] = 20;
+  m_polyFindRange[2] = 10;
+}
+
+bool Sapphire::NaviProvider::init()
+{
+  auto meshesFolder = std::experimental::filesystem::path( "navi" );
+  auto meshFolder = meshesFolder / std::experimental::filesystem::path( m_internalName );
+
+  if( std::experimental::filesystem::exists( meshFolder ) )
+  {
+    auto baseMesh = meshFolder / std::experimental::filesystem::path( m_internalName + ".nav" );
+
+    loadMesh( baseMesh.string() );
+
+    initQuery();
+
+    return true;
+  }
+
+  return false;
+}
+
+bool Sapphire::NaviProvider::hasNaviMesh() const
+{
+  return m_naviMesh != nullptr;
+}
+
+void Sapphire::NaviProvider::initQuery()
+{
+  if( m_naviMeshQuery != nullptr )
+    dtFreeNavMeshQuery( m_naviMeshQuery );
+
+  m_naviMeshQuery = dtAllocNavMeshQuery();
+  m_naviMeshQuery->init( m_naviMesh, 2048 );
+}
+
+int Sapphire::NaviProvider::fixupCorridor( dtPolyRef* path, const int npath, const int maxPath,
+                          const dtPolyRef* visited, const int nvisited )
+{
+  int furthestPath = -1;
+  int furthestVisited = -1;
+
+  // Find furthest common polygon.
+  for( int i = npath - 1; i >= 0; --i )
+  {
+    bool found = false;
+    for( int j = nvisited - 1; j >= 0; --j )
+    {
+      if( path[i] == visited[j] )
+      {
+        furthestPath = i;
+        furthestVisited = j;
+        found = true;
+      }
+    }
+    if( found )
+      break;
+  }
+
+  // If no intersection found just return current path. 
+  if( furthestPath == -1 || furthestVisited == -1 )
+    return npath;
+
+  // Concatenate paths.	
+
+  // Adjust beginning of the buffer to include the visited.
+  const int req = nvisited - furthestVisited;
+  const int orig = rcMin( furthestPath + 1, npath );
+  int size = rcMax( 0, npath - orig );
+  if( req + size > maxPath )
+    size = maxPath - req;
+  if( size )
+    memmove( path + req, path + orig, size * sizeof( dtPolyRef ) );
+
+  // Store visited
+  for( int i = 0; i < req; ++i )
+    path[i] = visited[( nvisited - 1 ) - i];
+
+  return req + size;
+}
+
+int Sapphire::NaviProvider::fixupShortcuts( dtPolyRef* path, int npath, dtNavMeshQuery* navQuery )
+{
+  if( npath < 3 )
+    return npath;
+
+  // Get connected polygons
+  static const int maxNeis = 16;
+  dtPolyRef neis[maxNeis];
+  int nneis = 0;
+
+  const dtMeshTile* tile = 0;
+  const dtPoly* poly = 0;
+  if( dtStatusFailed( navQuery->getAttachedNavMesh()->getTileAndPolyByRef( path[0], &tile, &poly ) ) )
+    return npath;
+
+  for( unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next )
+  {
+    const dtLink* link = &tile->links[k];
+    if( link->ref != 0 )
+    {
+      if( nneis < maxNeis )
+        neis[nneis++] = link->ref;
+    }
+  }
+
+  // If any of the neighbour polygons is within the next few polygons
+  // in the path, short cut to that polygon directly.
+  static const int maxLookAhead = 6;
+  int cut = 0;
+  for( int i = dtMin( maxLookAhead, npath ) - 1; i > 1 && cut == 0; i-- ) {
+    for( int j = 0; j < nneis; j++ )
+    {
+      if( path[i] == neis[j] ) {
+        cut = i;
+        break;
+      }
+    }
+  }
+  if( cut > 1 )
+  {
+    int offset = cut - 1;
+    npath -= offset;
+    for( int i = 1; i < npath; i++ )
+      path[i] = path[i + offset];
+  }
+
+  return npath;
+}
+
+bool Sapphire::NaviProvider::inRange( const float* v1, const float* v2, const float r, const float h )
+{
+  const float dx = v2[0] - v1[0];
+  const float dy = v2[1] - v1[1];
+  const float dz = v2[2] - v1[2];
+  return ( dx*dx + dz * dz ) < r*r && fabsf( dy ) < h;
+}
+
+bool Sapphire::NaviProvider::getSteerTarget( dtNavMeshQuery* navQuery, const float* startPos, const float* endPos,
+                            const float minTargetDist,
+                            const dtPolyRef* path, const int pathSize,
+                            float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef,
+                            float* outPoints, int* outPointCount )
+{
+  // Find steer target.
+  static const int MAX_STEER_POINTS = 3;
+  float steerPath[MAX_STEER_POINTS * 3];
+  unsigned char steerPathFlags[MAX_STEER_POINTS];
+  dtPolyRef steerPathPolys[MAX_STEER_POINTS];
+  int nsteerPath = 0;
+  navQuery->findStraightPath( startPos, endPos, path, pathSize,
+                              steerPath, steerPathFlags, steerPathPolys, &nsteerPath, MAX_STEER_POINTS );
+  if( !nsteerPath )
+    return false;
+
+  if( outPoints && outPointCount )
+  {
+    *outPointCount = nsteerPath;
+    for( int i = 0; i < nsteerPath; ++i )
+      dtVcopy( &outPoints[i * 3], &steerPath[i * 3] );
+  }
+
+
+  // Find vertex far enough to steer to.
+  int ns = 0;
+  while( ns < nsteerPath )
+  {
+    // Stop at Off-Mesh link or when point is further than slop away.
+    if( ( steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION ) ||
+        !inRange( &steerPath[ns * 3], startPos, minTargetDist, 1000.0f ) )
+      break;
+    ns++;
+  }
+  // Failed to find good point to steer to.
+  if( ns >= nsteerPath )
+    return false;
+
+  dtVcopy( steerPos, &steerPath[ns * 3] );
+  steerPos[1] = startPos[1];
+  steerPosFlag = steerPathFlags[ns];
+  steerPosRef = steerPathPolys[ns];
+
+  return true;
+}
+
+std::vector< Sapphire::Common::FFXIVARR_POSITION3 > Sapphire::NaviProvider::findFollowPath( Common::FFXIVARR_POSITION3 startPos, Common::FFXIVARR_POSITION3 endPos )
+{
+  if( !m_naviMesh || !m_naviMeshQuery )
+    throw std::runtime_error( "No navimesh loaded" );
+
+  auto resultCoords = std::vector< Common::FFXIVARR_POSITION3 >();
+
+  dtPolyRef startRef, endRef = 0;
+
+  float spos[3] = {startPos.x, startPos.y, startPos.z};
+  float epos[3] = {endPos.x, endPos.y, endPos.z};
+
+  dtQueryFilter filter;
+  filter.setIncludeFlags( 0xffff );
+  filter.setExcludeFlags( 0 );
+
+  m_naviMeshQuery->findNearestPoly( spos, m_polyFindRange, &filter, &startRef, 0 );
+  m_naviMeshQuery->findNearestPoly( epos, m_polyFindRange, &filter, &endRef, 0 );
+
+  // Couldn't find any close polys to navigate from
+  if( !startRef || !endRef )
+    return resultCoords;
+
+  dtPolyRef polys[MAX_POLYS];
+  int numPolys = 0;
+
+  m_naviMeshQuery->findPath( startRef, endRef, spos, epos, &filter, polys, &numPolys, MAX_POLYS );
+
+  // Check if we got polys back for navigation
+  if( numPolys )
+  {
+    // Iterate over the path to find smooth path on the detail mesh surface.
+    memcpy( polys, polys, sizeof( dtPolyRef )*numPolys );
+    int npolys = numPolys;
+
+    float iterPos[3], targetPos[3];
+    m_naviMeshQuery->closestPointOnPoly( startRef, spos, iterPos, 0 );
+    m_naviMeshQuery->closestPointOnPoly( polys[npolys - 1], epos, targetPos, 0 );
+
+    Logger::debug("IterPos: {0} {1} {2}; TargetPos: {3} {4} {5}", iterPos[0], iterPos[1], iterPos[2], targetPos[0], targetPos[1], targetPos[2]);
+
+    static const float STEP_SIZE = 1.2f;
+    static const float SLOP = 0.15f;
+
+    int numSmoothPath = 0;
+    float smoothPath[MAX_SMOOTH * 3];
+
+    dtVcopy( &smoothPath[numSmoothPath * 3], iterPos );
+    numSmoothPath++;
+
+    // Move towards target a small advancement at a time until target reached or
+    // when ran out of memory to store the path.
+    while( npolys && numSmoothPath < MAX_SMOOTH )
+    {
+      // Find location to steer towards.
+      float steerPos[3];
+      unsigned char steerPosFlag;
+      dtPolyRef steerPosRef;
+
+      if( !getSteerTarget( m_naviMeshQuery, iterPos, targetPos, SLOP,
+                           polys, npolys, steerPos, steerPosFlag, steerPosRef ) )
+        break;
+
+      bool endOfPath = ( steerPosFlag & DT_STRAIGHTPATH_END ) ? true : false;
+      bool offMeshConnection = ( steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION ) ? true : false;
+
+      // Find movement delta.
+      float delta[3], len;
+      dtVsub( delta, steerPos, iterPos );
+      len = dtMathSqrtf( dtVdot( delta, delta ) );
+      // If the steer target is end of path or off-mesh link, do not move past the location.
+      if( ( endOfPath || offMeshConnection ) && len < STEP_SIZE )
+        len = 1;
+      else
+        len = STEP_SIZE / len;
+      float moveTgt[3];
+      dtVmad( moveTgt, iterPos, delta, len );
+
+      // Move
+      float result[3];
+      dtPolyRef visited[16];
+      int nvisited = 0;
+      m_naviMeshQuery->moveAlongSurface( polys[0], iterPos, moveTgt, &filter,
+                                         result, visited, &nvisited, 16 );
+
+      npolys = fixupCorridor( polys, npolys, MAX_POLYS, visited, nvisited );
+      npolys = fixupShortcuts( polys, npolys, m_naviMeshQuery );
+
+      float h = 0;
+      m_naviMeshQuery->getPolyHeight( polys[0], result, &h );
+      result[1] = h;
+      dtVcopy( iterPos, result );
+
+      // Handle end of path and off-mesh links when close enough.
+      if( endOfPath && inRange( iterPos, steerPos, SLOP, 1.0f ) )
+      {
+        // Reached end of path.
+        dtVcopy( iterPos, targetPos );
+        if( numSmoothPath < MAX_SMOOTH )
+        {
+          dtVcopy( &smoothPath[numSmoothPath * 3], iterPos );
+          numSmoothPath++;
+        }
+        break;
+      }
+      else if( offMeshConnection && inRange( iterPos, steerPos, SLOP, 1.0f ) )
+      {
+        // Reached off-mesh connection.
+        float startPos[3], endPos[3];
+
+        // Advance the path up to and over the off-mesh connection.
+        dtPolyRef prevRef = 0, polyRef = polys[0];
+        int npos = 0;
+        while( npos < npolys && polyRef != steerPosRef )
+        {
+          prevRef = polyRef;
+          polyRef = polys[npos];
+          npos++;
+        }
+        for( int i = npos; i < npolys; ++i )
+          polys[i - npos] = polys[i];
+        npolys -= npos;
+
+        // Handle the connection.
+        dtStatus status = m_naviMesh->getOffMeshConnectionPolyEndPoints( prevRef, polyRef, startPos, endPos );
+        if( dtStatusSucceed( status ) )
+        {
+          if( numSmoothPath < MAX_SMOOTH )
+          {
+            dtVcopy( &smoothPath[numSmoothPath * 3], startPos );
+            numSmoothPath++;
+            // Hack to make the dotted path not visible during off-mesh connection.
+            if( numSmoothPath & 1 )
+            {
+              dtVcopy( &smoothPath[numSmoothPath * 3], startPos );
+              numSmoothPath++;
+            }
+          }
+          // Move position at the other side of the off-mesh link.
+          dtVcopy( iterPos, endPos );
+          float eh = 0.0f;
+          m_naviMeshQuery->getPolyHeight( polys[0], iterPos, &eh );
+          iterPos[1] = eh;
+        }
+      }
+
+      // Store results.
+      if( numSmoothPath < MAX_SMOOTH )
+      {
+        dtVcopy( &smoothPath[numSmoothPath * 3], iterPos );
+        numSmoothPath++;
+      }
+    }
+
+    for( int i = 0; i < numSmoothPath; i += 3 )
+    {
+      resultCoords.push_back( Common::FFXIVARR_POSITION3{ smoothPath[i], smoothPath[i + 1], smoothPath[i + 2] } );
+    }
+  }
+
+  return resultCoords;
+}
+
+void Sapphire::NaviProvider::loadMesh( std::string path )
+{
+  FILE* fp = fopen( path.c_str(), "rb" );
+  if( !fp )
+    throw std::runtime_error( "Could open navimesh file" );
+
+  // Read header.
+  NavMeshSetHeader header;
+
+  size_t readLen = fread( &header, sizeof( NavMeshSetHeader ), 1, fp );
+  if( readLen != 1 )
+  {
+    fclose( fp );
+    throw std::runtime_error( "Could not read NavMeshSetHeader" );
+  }
+
+  if( header.magic != NAVMESHSET_MAGIC )
+  {
+    fclose( fp );
+    throw std::runtime_error( "Not a NavMeshSet" );
+  }
+
+  if( header.version != NAVMESHSET_VERSION )
+  {
+    fclose( fp );
+    throw std::runtime_error( "Invalid NavMeshSet version" );
+  }
+
+  if( !m_naviMesh )
+  {
+    m_naviMesh = dtAllocNavMesh();
+    if( !m_naviMesh )
+    {
+      fclose( fp );
+      throw std::runtime_error( "Could not allocate dtNavMesh" );
+    }
+
+    dtStatus status = m_naviMesh->init( &header.params );
+    if( dtStatusFailed( status ) )
+    {
+      fclose( fp );
+      throw std::runtime_error( "Could not initialize dtNavMesh" );
+    }
+  }
+
+  // Read tiles.
+  for( int i = 0; i < header.numTiles; ++i )
+  {
+    NavMeshTileHeader tileHeader;
+    readLen = fread( &tileHeader, sizeof( tileHeader ), 1, fp );
+    if( readLen != 1 )
+    {
+      fclose( fp );
+      throw std::runtime_error( "Could not read NavMeshTileHeader" );
+    }
+
+    if( !tileHeader.tileRef || !tileHeader.dataSize )
+      break;
+
+    unsigned char* data = (unsigned char*)dtAlloc( tileHeader.dataSize, DT_ALLOC_PERM );
+    if( !data ) break;
+    memset( data, 0, tileHeader.dataSize );
+    readLen = fread( data, tileHeader.dataSize, 1, fp );
+    if( readLen != 1 )
+    {
+      dtFree( data );
+      fclose( fp );
+      throw std::runtime_error( "Could not read tile data" );
+    }
+
+    m_naviMesh->addTile( data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0 );
+  }
+
+  fclose( fp );
+}

src/world/Navi/NaviProvider.h

@@ -0,0 +1,67 @@
+#ifndef _NAVIPROVIDER_H_
+#define _NAVIPROVIDER_H_
+
+#include <Common.h>
+#include "ForwardsZone.h"
+#include <recastnavigation/Detour/Include/DetourNavMesh.h>
+#include <recastnavigation/Detour/Include/DetourNavMeshQuery.h>
+
+namespace Sapphire
+{
+
+  class NaviProvider
+  {
+
+    static const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET';
+    static const int NAVMESHSET_VERSION = 1;
+
+    struct NavMeshSetHeader
+    {
+      int magic;
+      int version;
+      int numTiles;
+      dtNavMeshParams params;
+    };
+
+    struct NavMeshTileHeader
+    {
+      dtTileRef tileRef;
+      int dataSize;
+    };
+
+    static const int MAX_POLYS = 256;
+	  static const int MAX_SMOOTH = 2048;
+
+  public:
+    NaviProvider( const std::string internalName );
+
+    bool init();
+    void loadMesh( std::string path );
+    void initQuery();
+
+    void toDetourPos(const Common::FFXIVARR_POSITION3 position, float* out);
+    Sapphire::Common::FFXIVARR_POSITION3 toGamePos( float* pos );
+
+    std::vector< Sapphire::Common::FFXIVARR_POSITION3 > findFollowPath(Common::FFXIVARR_POSITION3 startPos, Common::FFXIVARR_POSITION3 endPos);
+
+    bool hasNaviMesh() const;
+
+  protected:
+    std::string m_internalName;
+
+    dtNavMesh* m_naviMesh;
+    dtNavMeshQuery* m_naviMeshQuery;
+
+    float m_polyFindRange[3];
+
+  private:
+    static int fixupCorridor( dtPolyRef* path, const int npath, const int maxPath, const dtPolyRef* visited, const int nvisited );
+    static int fixupShortcuts( dtPolyRef* path, int npath, dtNavMeshQuery* navQuery );
+    inline static bool inRange( const float* v1, const float* v2, const float r, const float h );
+    static bool getSteerTarget( dtNavMeshQuery* navQuery, const float* startPos, const float* endPos, const float minTargetDist, const dtPolyRef* path, const int pathSize, float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef, float* outPoints = 0, int* outPointCount = 0 );
+
+  };
+
+}
+
+#endif

src/world/ServerMgr.cpp

@@ -41,6 +41,7 @@
 #include "Manager/ItemMgr.h"
 #include "Manager/MarketMgr.h"
 #include "Manager/RNGMgr.h"
+#include "Manager/NaviMgr.h"
 
 using namespace Sapphire::World::Manager;
 
@@ -166,6 +167,9 @@ void Sapphire::World::ServerMgr::run( int32_t argc, char* argv[] )
 
   loadBNpcTemplates();
 
+  auto pNaviMgr = std::make_shared< Manager::NaviMgr >( framework() );
+  framework()->set< Manager::NaviMgr >( pNaviMgr );
+
   Logger::info( "TerritoryMgr: Setting up zones" );
   auto pTeriMgr = std::make_shared< Manager::TerritoryMgr >( framework() );
   auto pHousingMgr = std::make_shared< Manager::HousingMgr >( framework() );
@@ -451,10 +455,10 @@ void Sapphire::World::ServerMgr::loadBNpcTemplates()
     auto look = res->getBlobVector( 12 );
     auto models = res->getBlobVector( 13 );
 
-    auto bnpcTemplate = std::make_shared< Entity::BNpcTemplate >(
-                                              id, bNPCBaseId, bNPCNameId, mainWeaponModel, secWeaponModel,
-                                              aggressionMode, enemyType, 0, pose, modelChara, displayFlags,
-                                              reinterpret_cast< uint32_t* >( &models[ 0 ] ),
+    auto bnpcTemplate = std::make_shared< Entity::BNpcTemplate >(
+                                              id, bNPCBaseId, bNPCNameId, mainWeaponModel, secWeaponModel,
+                                              aggressionMode, enemyType, 0, pose, modelChara, displayFlags,
+                                              reinterpret_cast< uint32_t* >( &models[ 0 ] ),
                                               reinterpret_cast< uint8_t* >( &look[ 0 ] ) );
 
     m_bNpcTemplateMap[ name ] = bnpcTemplate;

src/world/Territory/Zone.cpp

@@ -831,3 +831,4 @@ void Sapphire::Zone::updateSpawnPoints()
   }
 
 }
+