BVB Source Codes

CRYENGINE Show NavigationSystem.cpp Source code

Return Download CRYENGINE: download NavigationSystem.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. #include "StdAfx.h"
  4. #include "NavigationSystem.h"
  5. #include "../MNM/TileGenerator.h"
  6. #include "../MNM/NavMesh.h"
  7. #include "DebugDrawContext.h"
  8. #include "MNMPathfinder.h"
  9. #include <CryThreading/IJobManager_JobDelegator.h>
  10. #include <CryCore/Platform/CryWindows.h>
  11.  
  12. // BAI navigation file version history
  13. // Changes in version 9
  14. //  - Navigation volumes storage is changed:
  15. //    * all used navigation volumes are saved (including exclusion volumes, which were missing before);
  16. //    * navigation area names saved together with volume data;
  17. //    * volumes stored only onces, instead of storing them together with each mesh.
  18. // Changes in version 8
  19. //  - struct MNM::Tile::STriangle layout is changed - now it has triangle flags
  20. #define BAI_NAVIGATION_FILE_VERSION 9
  21.  
  22. #define MAX_NAME_LENGTH             512
  23. #if defined(SW_NAVMESH_USE_GUID)
  24.         #define BAI_NAVIGATION_GUID_FLAG  (1 << 31)
  25. #else
  26.         #define BAI_NAVIGATION_GUID_FLAG  (1 << 30)
  27. #endif
  28.  
  29. //#pragma optimize("", off)
  30. //#pragma inline_depth(0)
  31.  
  32. #if !defined(_RELEASE)
  33.         #define NAVIGATION_SYSTEM_CONSOLE_AUTOCOMPLETE
  34. #endif
  35.  
  36. #if defined NAVIGATION_SYSTEM_CONSOLE_AUTOCOMPLETE
  37.  
  38.         #include <CrySystem/IConsole.h>
  39.  
  40. struct SAgentTypeListAutoComplete : public IConsoleArgumentAutoComplete
  41. {
  42.         virtual int GetCount() const
  43.         {
  44.                 assert(gAIEnv.pNavigationSystem);
  45.                 return static_cast<int>(gAIEnv.pNavigationSystem->GetAgentTypeCount());
  46.         }
  47.  
  48.         virtual const char* GetValue(int nIndex) const
  49.         {
  50.                 NavigationSystem* pNavigationSystem = gAIEnv.pNavigationSystem;
  51.                 assert(pNavigationSystem);
  52.                 return pNavigationSystem->GetAgentTypeName(pNavigationSystem->GetAgentTypeID(nIndex));
  53.         }
  54. };
  55.  
  56. SAgentTypeListAutoComplete s_agentTypeListAutoComplete;
  57.  
  58. #endif
  59.  
  60. enum { MaxTaskCountPerWorkerThread = 12, };
  61. enum { MaxVolumeDefCopyCount = 8 }; // volume copies for access in other threads
  62.  
  63. #if NAVIGATION_SYSTEM_PC_ONLY
  64. void GenerateTileJob(MNM::CTileGenerator::Params params, volatile uint16* state, MNM::STile* tile, uint32* hashValue)
  65. {
  66.         if (*state != NavigationSystem::TileTaskResult::Failed)
  67.         {
  68.                 MNM::CTileGenerator generator;
  69.                 bool result = generator.Generate(params, *tile, hashValue);
  70.                 if (result)
  71.                         *state = NavigationSystem::TileTaskResult::Completed;
  72.                 else if (((params.flags & MNM::CTileGenerator::Params::NoHashTest) == 0) && (*hashValue == params.hashValue))
  73.                         *state = NavigationSystem::TileTaskResult::NoChanges;
  74.                 else
  75.                         *state = NavigationSystem::TileTaskResult::Failed;
  76.         }
  77. }
  78.  
  79. DECLARE_JOB("NavigationGeneration", NavigationGenerationJob, GenerateTileJob);
  80. #endif
  81.  
  82. uint32 NameHash(const char* name)
  83. {
  84.         uint32 len = strlen(name);
  85.         uint32 hash = 0;
  86.  
  87.         for (uint32 i = 0; i < len; ++i)
  88.         {
  89.                 hash += (uint8)(CryStringUtils::toLowerAscii(name[i]));
  90.                 hash += (hash << 10);
  91.                 hash ^= (hash >> 6);
  92.         }
  93.  
  94.         hash += (hash << 3);
  95.         hash ^= (hash >> 11);
  96.         hash += (hash << 15);
  97.  
  98.         return hash;
  99. }
  100.  
  101. bool ShouldBeConsideredByVoxelizer(IPhysicalEntity& physicalEntity, uint32& flags)
  102. {
  103.         if (physicalEntity.GetType() == PE_WHEELEDVEHICLE)
  104.         {
  105.                 return false;
  106.         }
  107.  
  108.         bool considerMass = (physicalEntity.GetType() == PE_RIGID);
  109.         if (!considerMass)
  110.         {
  111.                 pe_status_pos sp;
  112.                 if (physicalEntity.GetStatus(&sp))
  113.                 {
  114.                         considerMass = (sp.iSimClass == SC_ACTIVE_RIGID) || (sp.iSimClass == SC_SLEEPING_RIGID);
  115.                 }
  116.         }
  117.  
  118.         if (considerMass)
  119.         {
  120.                 pe_status_dynamics dyn;
  121.                 if (!physicalEntity.GetStatus(&dyn))
  122.                 {
  123.                         return false;
  124.                 }
  125.  
  126.                 if (dyn.mass > 1e-6f)
  127.                 {
  128.                         return false;
  129.                 }
  130.         }
  131.  
  132.         return true;
  133. }
  134.  
  135. //////////////////////////////////////////////////////////////////////////
  136. //////////////////////////////////////////////////////////////////////////
  137.  
  138. NavigationSystem::NavigationSystem(const char* configName)
  139.         : m_configName(configName)
  140.         , m_throughput(0.0f)
  141.         , m_cacheHitRate(0.0f)
  142.         , m_free(0)
  143.         , m_state(Idle)
  144.         , m_meshes(256)                 //Same size of meshes, off-mesh and islandConnections elements
  145.         , m_offMeshNavigationManager(256)
  146.         , m_islandConnectionsManager()
  147.         , m_volumes(512)
  148.         , m_worldAABB(AABB::RESET)
  149.         , m_volumeDefCopy(MaxVolumeDefCopyCount, VolumeDefCopy())
  150.         , m_listenersList(10)
  151.         , m_users(10)
  152.         , m_configurationVersion(0)
  153.         , m_isNavigationUpdatePaused(false)
  154.         , m_tileGeneratorExtensionsContainer()
  155. {
  156.         SetupTasks();
  157.  
  158.         m_worldMonitor = WorldMonitor(functor(*this, &NavigationSystem::WorldChanged));
  159.  
  160.         ReloadConfig();
  161.  
  162.         m_pEditorBackgroundUpdate = new NavigationSystemBackgroundUpdate(*this);
  163.  
  164. #ifdef SEG_WORLD
  165.         if (ISystemEventDispatcher* pSystemEventDispatcher = gEnv->pSystem->GetISystemEventDispatcher())
  166.         {
  167.                 pSystemEventDispatcher->RegisterListener(this);
  168.         }
  169. #endif
  170.  
  171. #ifdef NAVIGATION_SYSTEM_CONSOLE_AUTOCOMPLETE
  172.         gEnv->pConsole->RegisterAutoComplete("ai_debugMNMAgentType", &s_agentTypeListAutoComplete);
  173. #endif
  174. }
  175.  
  176. NavigationSystem::~NavigationSystem()
  177. {
  178.         StopWorldMonitoring();
  179.  
  180.         Clear();
  181.  
  182.         SAFE_DELETE(m_pEditorBackgroundUpdate);
  183.  
  184. #ifdef SEG_WORLD
  185.         if (ISystemEventDispatcher* pSystemEventDispatcher = gEnv->pSystem->GetISystemEventDispatcher())
  186.         {
  187.                 pSystemEventDispatcher->RemoveListener(this);
  188.         }
  189. #endif
  190.  
  191. #ifdef NAVIGATION_SYSTEM_CONSOLE_AUTOCOMPLETE
  192.         gEnv->pConsole->UnRegisterAutoComplete("ai_debugMNMAgentType");
  193. #endif
  194. }
  195.  
  196. NavigationAgentTypeID NavigationSystem::CreateAgentType(const char* name, const CreateAgentTypeParams& params)
  197. {
  198.         assert(name);
  199.         AgentTypes::const_iterator it = m_agentTypes.begin();
  200.         AgentTypes::const_iterator end = m_agentTypes.end();
  201.  
  202.         for (; it != end; ++it)
  203.         {
  204.                 const AgentType& agentType = *it;
  205.  
  206.                 if (!agentType.name.compareNoCase(name))
  207.                 {
  208.                         AIWarning("Trying to create NavigationSystem AgentType with duplicate name '%s'!", name);
  209.                         assert(0);
  210.                         return NavigationAgentTypeID();
  211.                 }
  212.         }
  213.  
  214.         m_agentTypes.resize(m_agentTypes.size() + 1);
  215.         AgentType& agentType = m_agentTypes.back();
  216.  
  217.         agentType.name = name;
  218.         agentType.settings.voxelSize = params.voxelSize;
  219.         agentType.settings.radiusVoxelCount = params.radiusVoxelCount;
  220.         agentType.settings.climbableVoxelCount = params.climbableVoxelCount;
  221.         agentType.settings.climbableInclineGradient = params.climbableInclineGradient;
  222.         agentType.settings.climbableStepRatio = params.climbableStepRatio;
  223.         agentType.meshEntityCallback = functor(ShouldBeConsideredByVoxelizer);
  224.         agentType.settings.heightVoxelCount = params.heightVoxelCount;
  225.         agentType.settings.maxWaterDepthVoxelCount = params.maxWaterDepthVoxelCount;
  226.  
  227.         return NavigationAgentTypeID(m_agentTypes.size());
  228. }
  229.  
  230. NavigationAgentTypeID NavigationSystem::GetAgentTypeID(const char* name) const
  231. {
  232.         assert(name);
  233.  
  234.         AgentTypes::const_iterator it = m_agentTypes.begin();
  235.         AgentTypes::const_iterator end = m_agentTypes.end();
  236.  
  237.         for (; it != end; ++it)
  238.         {
  239.                 const AgentType& agentType = *it;
  240.  
  241.                 if (!agentType.name.compareNoCase(name))
  242.                         return NavigationAgentTypeID((uint32)(std::distance(m_agentTypes.begin(), it) + 1));
  243.         }
  244.  
  245.         return NavigationAgentTypeID();
  246. }
  247.  
  248. NavigationAgentTypeID NavigationSystem::GetAgentTypeID(size_t index) const
  249. {
  250.         if (index < m_agentTypes.size())
  251.                 return NavigationAgentTypeID(index + 1);
  252.  
  253.         return NavigationAgentTypeID();
  254. }
  255.  
  256. const char* NavigationSystem::GetAgentTypeName(NavigationAgentTypeID agentTypeID) const
  257. {
  258.         if (agentTypeID && agentTypeID <= m_agentTypes.size())
  259.                 return m_agentTypes[agentTypeID - 1].name.c_str();
  260.  
  261.         return 0;
  262. }
  263.  
  264. size_t NavigationSystem::GetAgentTypeCount() const
  265. {
  266.         return m_agentTypes.size();
  267. }
  268.  
  269. bool NavigationSystem::GetAgentTypeProperties(const NavigationAgentTypeID agentTypeID, AgentType& agentTypeProperties) const
  270. {
  271.         if (agentTypeID && agentTypeID <= m_agentTypes.size())
  272.         {
  273.                 agentTypeProperties = m_agentTypes[agentTypeID - 1];
  274.                 return true;
  275.         }
  276.  
  277.         return false;
  278. }
  279.  
  280. inline size_t NearestFactor(size_t n, size_t f)
  281. {
  282.         while (n % f)
  283.                 ++f;
  284.  
  285.         return f;
  286. }
  287. #ifdef SW_NAVMESH_USE_GUID
  288. NavigationMeshID NavigationSystem::CreateMesh(const char* name, NavigationAgentTypeID agentTypeID,
  289.                                               const CreateMeshParams& params, NavigationMeshGUID guid)
  290. {
  291.         MeshMap::iterator it = m_swMeshes.find(guid);
  292.         if (it != m_swMeshes.end())
  293.         {
  294.                 return it->second;
  295.         }
  296.         NavigationMeshID id(++m_nextFreeMeshId);
  297.         id = CreateMesh(name, agentTypeID, params, id, guid);
  298.         m_swMeshes.insert(std::pair<NavigationMeshGUID, NavigationMeshID>(guid, id));
  299.         return id;
  300. }
  301. #else
  302. NavigationMeshID NavigationSystem::CreateMesh(const char* name, NavigationAgentTypeID agentTypeID,
  303.                                               const CreateMeshParams& params)
  304. {
  305.         return CreateMesh(name, agentTypeID, params, NavigationMeshID(0));
  306. }
  307. #endif
  308. #ifdef SW_NAVMESH_USE_GUID
  309. NavigationMeshID NavigationSystem::CreateMesh(const char* name, NavigationAgentTypeID agentTypeID,
  310.                                               const CreateMeshParams& params, NavigationMeshID requestedID, NavigationMeshGUID guid)
  311. #else
  312. NavigationMeshID NavigationSystem::CreateMesh(const char* name, NavigationAgentTypeID agentTypeID,
  313.                                               const CreateMeshParams& params, NavigationMeshID requestedID)
  314. #endif
  315. {
  316.         assert(name && agentTypeID && agentTypeID <= m_agentTypes.size());
  317.  
  318.         if (agentTypeID && (agentTypeID <= m_agentTypes.size()))
  319.         {
  320.                 AgentType& agentType = m_agentTypes[agentTypeID - 1];
  321.  
  322.                 MNM::CNavMesh::SGridParams paramsGrid;
  323.                 paramsGrid.tileSize = params.tileSize;
  324.                 paramsGrid.voxelSize = agentType.settings.voxelSize;
  325.                 paramsGrid.tileCount = params.tileCount;
  326.  
  327.                 paramsGrid.voxelSize.x = NearestFactor(paramsGrid.tileSize.x * 1000, (size_t)(paramsGrid.voxelSize.x * 1000)) * 0.001f;
  328.                 paramsGrid.voxelSize.y = NearestFactor(paramsGrid.tileSize.y * 1000, (size_t)(paramsGrid.voxelSize.y * 1000)) * 0.001f;
  329.                 paramsGrid.voxelSize.z = NearestFactor(paramsGrid.tileSize.z * 1000, (size_t)(paramsGrid.voxelSize.z * 1000)) * 0.001f;
  330.  
  331.                 if (!paramsGrid.voxelSize.IsEquivalent(agentType.settings.voxelSize, 0.0001f))
  332.                 {
  333.                         AIWarning("VoxelSize (%.3f, %.3f, %.3f) for agent '%s' adjusted to (%.3f, %.3f, %.3f)"
  334.                                   " - needs to be a factor of TileSize (%d, %d, %d)",
  335.                                   agentType.settings.voxelSize.x, agentType.settings.voxelSize.y, agentType.settings.voxelSize.z,
  336.                                   agentType.name.c_str(),
  337.                                   paramsGrid.voxelSize.x, paramsGrid.voxelSize.y, paramsGrid.voxelSize.z,
  338.                                   paramsGrid.tileSize.x, paramsGrid.tileSize.y, paramsGrid.tileSize.z);
  339.                 }
  340.  
  341.                 NavigationMeshID id = requestedID;
  342.                 if (requestedID == NavigationMeshID(0))
  343.                         id = NavigationMeshID(m_meshes.insert(NavigationMesh(agentTypeID)));
  344.                 else
  345.                         m_meshes.insert(requestedID, NavigationMesh(agentTypeID));
  346.                 NavigationMesh& mesh = m_meshes[id];
  347.                 mesh.navMesh.Init(paramsGrid);
  348.                 mesh.name = name;
  349.                 mesh.exclusions = agentType.exclusions;
  350.  
  351. #ifdef SW_NAVMESH_USE_GUID
  352.                 agentType.meshes.push_back(AgentType::MeshInfo(guid, id, NameHash(name)));
  353. #else
  354.                 agentType.meshes.push_back(AgentType::MeshInfo(id, NameHash(name)));
  355. #endif
  356.  
  357.                 m_offMeshNavigationManager.OnNavigationMeshCreated(id);
  358.  
  359.                 return id;
  360.         }
  361.  
  362.         return NavigationMeshID();
  363. }
  364.  
  365. void NavigationSystem::DestroyMesh(NavigationMeshID meshID)
  366. {
  367.         if (meshID && m_meshes.validate(meshID))
  368.         {
  369.                 NavigationMesh& mesh = m_meshes[meshID];
  370.  
  371.                 AILogComment("NavigationSystem::DestroyMesh meshID = %u '%s'", (unsigned int)meshID, mesh.name.c_str());
  372.  
  373.                 for (size_t t = 0; t < m_runningTasks.size(); ++t)
  374.                 {
  375.                         if (m_results[m_runningTasks[t]].meshID == meshID)
  376.                                 m_results[m_runningTasks[t]].state = TileTaskResult::Failed;
  377.                 }
  378.  
  379.                 for (size_t t = 0; t < m_runningTasks.size(); ++t)
  380.                 {
  381.                         if (m_results[m_runningTasks[t]].meshID == meshID)
  382.                                 gEnv->pJobManager->WaitForJob(m_results[m_runningTasks[t]].jobState);
  383.                 }
  384.  
  385.                 AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  386.                 AgentType::Meshes::iterator it = agentType.meshes.begin();
  387.                 AgentType::Meshes::iterator end = agentType.meshes.end();
  388.  
  389.                 for (; it != end; ++it)
  390.                 {
  391.                         if (it->id == meshID)
  392.                         {
  393.                                 std::swap(*it, agentType.meshes.back());
  394.                                 agentType.meshes.pop_back();
  395.                                 break;
  396.                         }
  397.                 }
  398.  
  399.                 TileTaskQueue::iterator qit = m_tileQueue.begin();
  400.                 TileTaskQueue::iterator qend = m_tileQueue.end();
  401.  
  402.                 for (; qit != qend; ++qit)
  403.                 {
  404.                         TileTask& task = *qit;
  405.                         if (task.meshID == meshID)
  406.                                 task.aborted = true;
  407.                 }
  408.  
  409.                 m_meshes.erase(meshID);
  410.  
  411.                 m_offMeshNavigationManager.OnNavigationMeshDestroyed(meshID);
  412.  
  413.                 ComputeWorldAABB();
  414.         }
  415. }
  416.  
  417. void NavigationSystem::SetMeshEntityCallback(NavigationAgentTypeID agentTypeID, const NavigationMeshEntityCallback& callback)
  418. {
  419.         if (agentTypeID && (agentTypeID <= m_agentTypes.size()))
  420.         {
  421.                 AgentType& agentType = m_agentTypes[agentTypeID - 1];
  422.  
  423.                 agentType.meshEntityCallback = callback;
  424.         }
  425. }
  426.  
  427. void NavigationSystem::AddMeshChangeCallback(NavigationAgentTypeID agentTypeID,
  428.                                              const NavigationMeshChangeCallback& callback)
  429. {
  430.         if (agentTypeID && (agentTypeID <= m_agentTypes.size()))
  431.         {
  432.                 AgentType& agentType = m_agentTypes[agentTypeID - 1];
  433.  
  434.                 stl::push_back_unique(agentType.callbacks, callback);
  435.         }
  436. }
  437.  
  438. void NavigationSystem::RemoveMeshChangeCallback(NavigationAgentTypeID agentTypeID,
  439.                                                 const NavigationMeshChangeCallback& callback)
  440. {
  441.         if (agentTypeID && (agentTypeID <= m_agentTypes.size()))
  442.         {
  443.                 AgentType& agentType = m_agentTypes[agentTypeID - 1];
  444.  
  445.                 stl::find_and_erase(agentType.callbacks, callback);
  446.         }
  447. }
  448.  
  449. #ifdef SW_NAVMESH_USE_GUID
  450. void NavigationSystem::SetMeshBoundaryVolume(NavigationMeshID meshID, NavigationVolumeID volumeID, NavigationVolumeGUID volumeGUID)
  451. #else
  452. void NavigationSystem::SetMeshBoundaryVolume(NavigationMeshID meshID, NavigationVolumeID volumeID)
  453. #endif
  454. {
  455.         if (meshID && m_meshes.validate(meshID))
  456.         {
  457.                 NavigationMesh& mesh = m_meshes[meshID];
  458.  
  459.                 if (mesh.boundary)
  460.                 {
  461.                         ++mesh.version;
  462.                 }
  463.  
  464.                 if (!m_volumes.validate(volumeID))
  465.                 {
  466.                         AIWarning("NavigationSystem::SetMeshBoundaryVolume: setting invalid volumeID %u for a mesh %u '%s'", (unsigned int)volumeID, (unsigned int)meshID, mesh.name.c_str());
  467.                         volumeID = NavigationVolumeID();
  468.                 }
  469.  
  470.                 AILogComment("NavigationSystem::SetMeshBoundaryVolume: set volumeID %u for a mesh %u '%s'", (unsigned int)volumeID, (unsigned int)meshID, mesh.name.c_str());
  471.  
  472.                 mesh.boundary = volumeID;
  473. #ifdef SW_NAVMESH_USE_GUID
  474.                 mesh.boundaryGUID = volumeGUID;
  475. #endif
  476.  
  477.                 ComputeWorldAABB();
  478.         }
  479. }
  480.  
  481. #ifdef SW_NAVMESH_USE_GUID
  482. NavigationVolumeID NavigationSystem::CreateVolume(Vec3* vertices, size_t vertexCount, float height, NavigationVolumeGUID guid)
  483. {
  484.         VolumeMap::iterator it = m_swVolumes.find(guid);
  485.         if (it != m_swVolumes.end())
  486.         {
  487.                 return it->second;
  488.         }
  489.         NavigationVolumeID id(++m_nextFreeVolumeId);
  490.         id = CreateVolume(vertices, vertexCount, height, id);
  491.         m_swVolumes.insert(std::pair<NavigationVolumeGUID, NavigationVolumeID>(guid, id));
  492.         return id;
  493. }
  494. #else
  495. NavigationVolumeID NavigationSystem::CreateVolume(Vec3* vertices, size_t vertexCount, float height)
  496. {
  497.         return CreateVolume(vertices, vertexCount, height, NavigationVolumeID(0));
  498. }
  499. #endif
  500.  
  501. NavigationVolumeID NavigationSystem::CreateVolume(Vec3* vertices, size_t vertexCount, float height, NavigationVolumeID requestedID)
  502. {
  503.         NavigationVolumeID id = requestedID;
  504.         if (requestedID == NavigationVolumeID(0))
  505.                 id = NavigationVolumeID(m_volumes.insert(NavigationBoundingVolume()));
  506.         else
  507.                 m_volumes.insert(requestedID, NavigationBoundingVolume());
  508.         assert(id != 0);
  509.         SetVolume(id, vertices, vertexCount, height);
  510.  
  511.         return id;
  512. }
  513.  
  514. void NavigationSystem::DestroyVolume(NavigationVolumeID volumeID)
  515. {
  516.         if (volumeID && m_volumes.validate(volumeID))
  517.         {
  518.                 NavigationBoundingVolume& volume = m_volumes[volumeID];
  519.  
  520.                 AgentTypes::iterator it = m_agentTypes.begin();
  521.                 AgentTypes::iterator end = m_agentTypes.end();
  522.  
  523.                 for (; it != end; ++it)
  524.                 {
  525.                         AgentType& agentType = *it;
  526.  
  527.                         stl::find_and_erase(agentType.exclusions, volumeID);
  528.  
  529.                         AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  530.                         AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  531.  
  532.                         for (; mit != mend; ++mit)
  533.                         {
  534.                                 const NavigationMeshID meshID = mit->id;
  535.                                 NavigationMesh& mesh = m_meshes[meshID];
  536.  
  537.                                 if (mesh.boundary == volumeID)
  538.                                 {
  539.                                         ++mesh.version;
  540.                                         continue;
  541.                                 }
  542.  
  543.                                 if (stl::find_and_erase(mesh.exclusions, volumeID))
  544.                                 {
  545.                                         QueueMeshUpdate(meshID, volume.aabb);
  546.                                         ++mesh.version;
  547.                                 }
  548.                         }
  549.                 }
  550.  
  551.                 if (gEnv->IsEditor())
  552.                 {
  553.                         m_volumesManager.InvalidateID(volumeID);
  554.                 }
  555.  
  556.                 m_volumes.erase(volumeID);
  557.         }
  558. }
  559.  
  560. void NavigationSystem::SetVolume(NavigationVolumeID volumeID, Vec3* vertices, size_t vertexCount, float height)
  561. {
  562.         assert(vertices);
  563.  
  564.         if (volumeID && m_volumes.validate(volumeID))
  565.         {
  566.                 bool recomputeAABB = false;
  567.  
  568.                 NavigationBoundingVolume newVolume;
  569.                 AABB aabbNew(AABB::RESET);
  570.  
  571.                 newVolume.vertices.reserve(vertexCount);
  572.  
  573.                 for (size_t i = 0; i < vertexCount; ++i)
  574.                 {
  575.                         aabbNew.Add(vertices[i]);
  576.                         newVolume.vertices.push_back(vertices[i]);
  577.                 }
  578.  
  579.                 aabbNew.Add(vertices[0] + Vec3(0.0f, 0.0f, height));
  580.  
  581.                 newVolume.height = height;
  582.                 newVolume.aabb = aabbNew;
  583.  
  584.                 NavigationBoundingVolume& volume = m_volumes[volumeID];
  585.  
  586.                 if (!volume.vertices.empty())
  587.                 {
  588.                         AgentTypes::const_iterator it = m_agentTypes.begin();
  589.                         AgentTypes::const_iterator end = m_agentTypes.end();
  590.  
  591.                         for (; it != end; ++it)
  592.                         {
  593.                                 const AgentType& agentType = *it;
  594.  
  595.                                 AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  596.                                 AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  597.  
  598.                                 for (; mit != mend; ++mit)
  599.                                 {
  600.                                         const NavigationMeshID meshID = mit->id;
  601.                                         NavigationMesh& mesh = m_meshes[meshID];
  602.  
  603.                                         if (mesh.boundary == volumeID)
  604.                                         {
  605.                                                 ++mesh.version;
  606.                                                 recomputeAABB = true;
  607.  
  608.                                                 QueueDifferenceUpdate(meshID, volume, newVolume);
  609.                                         }
  610.  
  611.                                         if (std::find(mesh.exclusions.begin(), mesh.exclusions.end(), volumeID) != mesh.exclusions.end())
  612.                                         {
  613.                                                 QueueMeshUpdate(meshID, volume.aabb);
  614.                                                 QueueMeshUpdate(meshID, aabbNew);
  615.                                                 ++mesh.version;
  616.                                         }
  617.                                 }
  618.                         }
  619.                 }
  620.  
  621.                 newVolume.Swap(volume);
  622.  
  623.                 // recompute the world bounding box after we have set the volume
  624.                 if (recomputeAABB)
  625.                         ComputeWorldAABB();
  626.         }
  627. }
  628.  
  629. bool NavigationSystem::ValidateVolume(NavigationVolumeID volumeID) const
  630. {
  631.         return m_volumes.validate(volumeID);
  632. }
  633.  
  634. NavigationVolumeID NavigationSystem::GetVolumeID(NavigationMeshID meshID) const
  635. {
  636.         // This function is used to retrieve the correct ID of the volume boundary connected to the mesh.
  637.         // After restoring the navigation data it could be that the cached volume id in the Sandbox SapeObject
  638.         // is different from the actual one connected to the shape.
  639.         if (meshID && m_meshes.validate(meshID))
  640.                 return m_meshes[meshID].boundary;
  641.         else
  642.                 return NavigationVolumeID();
  643. }
  644.  
  645. #ifdef SW_NAVMESH_USE_GUID
  646. void NavigationSystem::SetExclusionVolume(const NavigationAgentTypeID* agentTypeIDs, size_t agentTypeIDCount, NavigationVolumeID volumeID, NavigationVolumeGUID volumeGUID)
  647. #else
  648. void NavigationSystem::SetExclusionVolume(const NavigationAgentTypeID* agentTypeIDs, size_t agentTypeIDCount, NavigationVolumeID volumeID)
  649. #endif
  650. {
  651.         if (volumeID && m_volumes.validate(volumeID))
  652.         {
  653.                 bool recomputeAABB = false;
  654.  
  655.                 NavigationBoundingVolume& volume = m_volumes[volumeID];
  656.  
  657.                 AgentTypes::iterator it = m_agentTypes.begin();
  658.                 AgentTypes::iterator end = m_agentTypes.end();
  659.  
  660.                 for (; it != end; ++it)
  661.                 {
  662.                         AgentType& agentType = *it;
  663.  
  664.                         stl::find_and_erase(agentType.exclusions, volumeID);
  665.  
  666.                         AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  667.                         AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  668.  
  669.                         for (; mit != mend; ++mit)
  670.                         {
  671.                                 const NavigationMeshID meshID = mit->id;
  672.                                 NavigationMesh& mesh = m_meshes[meshID];
  673.  
  674.                                 if (stl::find_and_erase(mesh.exclusions, volumeID))
  675.                                 {
  676.                                         QueueMeshUpdate(meshID, volume.aabb);
  677.  
  678.                                         ++mesh.version;
  679.                                 }
  680.                         }
  681.                 }
  682.  
  683.                 for (size_t i = 0; i < agentTypeIDCount; ++i)
  684.                 {
  685.                         if (agentTypeIDs[i] && (agentTypeIDs[i] <= m_agentTypes.size()))
  686.                         {
  687.                                 AgentType& agentType = m_agentTypes[agentTypeIDs[i] - 1];
  688.  
  689.                                 agentType.exclusions.push_back(volumeID);
  690.  
  691.                                 AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  692.                                 AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  693.  
  694.                                 for (; mit != mend; ++mit)
  695.                                 {
  696.                                         const NavigationMeshID meshID = mit->id;
  697.                                         NavigationMesh& mesh = m_meshes[meshID];
  698.  
  699.                                         mesh.exclusions.push_back(volumeID);
  700. #ifdef SW_NAVMESH_USE_GUID
  701.                                         mesh.exclusionsGUID.push_back(volumeGUID);
  702. #endif
  703.  
  704.                                         if (mesh.boundary != volumeID)
  705.                                                 QueueMeshUpdate(meshID, volume.aabb);
  706.                                         else
  707.                                         {
  708.                                                 ++mesh.version;
  709.                                                 AILogComment("NavigationSystem::SetExclusionVolume: volumeID %u for a mesh %u '%s'", (unsigned int)volumeID, (unsigned int)meshID, mesh.name.c_str());
  710.                                                 mesh.boundary = NavigationVolumeID();
  711.                                                 recomputeAABB = true;
  712.                                         }
  713.                                 }
  714.                         }
  715.                 }
  716.  
  717.                 if (recomputeAABB)
  718.                         ComputeWorldAABB();
  719.         }
  720. }
  721.  
  722. /*
  723.    void NavigationSystem::AddMeshExclusionVolume(NavigationMeshID meshID, NavigationVolumeID volumeID)
  724.    {
  725.    if (meshID && volumeID && m_meshes.validate(meshID) && m_volumes.validate(volumeID))
  726.    {
  727.    NavigationMesh& mesh = m_meshes[meshID];
  728.  
  729.    if (stl::push_back_unique(mesh.exclusions, volumeID))
  730.    ++mesh.version;
  731.    }
  732.    }
  733.  
  734.    void NavigationSystem::RemoveMeshExclusionVolume(NavigationMeshID meshID, NavigationVolumeID volumeID)
  735.    {
  736.    if (meshID && volumeID && m_meshes.validate(meshID) && m_volumes.validate(volumeID))
  737.    {
  738.    NavigationMesh& mesh = m_meshes[meshID];
  739.  
  740.    if (stl::find_and_erase(mesh.exclusions, volumeID))
  741.    ++mesh.version;
  742.    }
  743.    }
  744.  */
  745. NavigationMeshID NavigationSystem::GetMeshID(const char* name, NavigationAgentTypeID agentTypeID) const
  746. {
  747.         assert(name && agentTypeID && (agentTypeID <= m_agentTypes.size()));
  748.  
  749.         if (agentTypeID && (agentTypeID <= m_agentTypes.size()))
  750.         {
  751.                 uint32 nameHash = NameHash(name);
  752.  
  753.                 const AgentType& agentType = m_agentTypes[agentTypeID - 1];
  754.                 AgentType::Meshes::const_iterator it = agentType.meshes.begin();
  755.                 AgentType::Meshes::const_iterator end = agentType.meshes.end();
  756.  
  757.                 for (; it != end; ++it)
  758.                 {
  759.                         if (it->name == nameHash)
  760.                                 return it->id;
  761.                 }
  762.         }
  763.  
  764.         return NavigationMeshID();
  765. }
  766.  
  767. const char* NavigationSystem::GetMeshName(NavigationMeshID meshID) const
  768. {
  769.         if (meshID && m_meshes.validate(meshID))
  770.         {
  771.                 const NavigationMesh& mesh = m_meshes[meshID];
  772.  
  773.                 return mesh.name.c_str();
  774.         }
  775.  
  776.         return 0;
  777. }
  778.  
  779. void NavigationSystem::SetMeshName(NavigationMeshID meshID, const char* name)
  780. {
  781.         if (meshID && m_meshes.validate(meshID))
  782.         {
  783.                 NavigationMesh& mesh = m_meshes[meshID];
  784.  
  785.                 mesh.name = name;
  786.  
  787.                 AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  788.                 AgentType::Meshes::iterator it = agentType.meshes.begin();
  789.                 AgentType::Meshes::iterator end = agentType.meshes.end();
  790.  
  791.                 for (; it != end; ++it)
  792.                 {
  793.                         if (it->id == meshID)
  794.                         {
  795.                                 it->name = NameHash(name);
  796.                                 break;
  797.                         }
  798.                 }
  799.         }
  800. }
  801.  
  802. void NavigationSystem::ResetAllNavigationSystemUsers()
  803. {
  804.         for (NavigationSystemUsers::Notifier notifier(m_users); notifier.IsValid(); notifier.Next())
  805.         {
  806.                 notifier->Reset();
  807.         }
  808. }
  809.  
  810. void NavigationSystem::WaitForAllNavigationSystemUsersCompleteTheirReadingAsynchronousTasks()
  811. {
  812.         for (NavigationSystemUsers::Notifier notifier(m_users); notifier.IsValid(); notifier.Next())
  813.         {
  814.                 notifier->CompleteRunningTasks();
  815.         }
  816. }
  817.  
  818. INavigationSystem::WorkingState NavigationSystem::GetState() const
  819. {
  820.         return m_state;
  821. }
  822.  
  823. void NavigationSystem::UpdateNavigationSystemUsersForSynchronousWritingOperations()
  824. {
  825.         for (NavigationSystemUsers::Notifier notifier(m_users); notifier.IsValid(); notifier.Next())
  826.         {
  827.                 notifier->UpdateForSynchronousWritingOperations();
  828.         }
  829. }
  830.  
  831. void NavigationSystem::UpdateNavigationSystemUsersForSynchronousOrAsynchronousReadingOperations()
  832. {
  833.         for (NavigationSystemUsers::Notifier notifier(m_users); notifier.IsValid(); notifier.Next())
  834.         {
  835.                 notifier->UpdateForSynchronousOrAsynchronousReadingOperation();
  836.         }
  837. }
  838.  
  839. void NavigationSystem::UpdateInternalNavigationSystemData(const bool blocking)
  840. {
  841. #if NAVIGATION_SYSTEM_PC_ONLY
  842.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  843.  
  844.         CRY_ASSERT_MESSAGE(m_pEditorBackgroundUpdate->IsRunning() == false, "Background update for editor is still running while the application has the focus!!");
  845.  
  846.         const bool editorBackgroundThreadRunning = m_pEditorBackgroundUpdate->IsRunning();
  847.         if (editorBackgroundThreadRunning)
  848.         {
  849.                 AIError("NavigationSystem - Editor background thread is still running, while the main thread is updating, skipping update");
  850.                 return;
  851.         }
  852.  
  853.         m_worldMonitor.FlushPendingAABBChanges();
  854.  
  855.         // Prevent multiple updates per frame
  856.         static int lastUpdateFrameID = 0;
  857.  
  858.         const int frameID = gEnv->nMainFrameID;
  859.         const bool doUpdate = (frameID != lastUpdateFrameID) && !(editorBackgroundThreadRunning);
  860.         if (doUpdate)
  861.         {
  862.                 lastUpdateFrameID = frameID;
  863.  
  864.                 const float frameTime = gEnv->pTimer->GetFrameTime();
  865.  
  866.                 UpdateMeshes(frameTime, blocking, gAIEnv.CVars.NavigationSystemMT != 0, false);
  867.         }
  868. #endif
  869. }
  870.  
  871. void NavigationSystem::UpdateInternalSubsystems()
  872. {
  873.         m_offMeshNavigationManager.ProcessQueuedRequests();
  874. }
  875.  
  876. INavigationSystem::WorkingState NavigationSystem::Update(bool blocking)
  877. {
  878.         // Pre update step. We need to request all our NavigationSystem users
  879.         // to complete all their reading jobs.
  880.         WaitForAllNavigationSystemUsersCompleteTheirReadingAsynchronousTasks();
  881.  
  882.         // step 1: update all the tasks that may write on the NavigationSystem data
  883.         UpdateInternalNavigationSystemData(blocking);
  884.         UpdateInternalSubsystems();
  885.  
  886.         // step 2: update all the writing tasks that needs to write some data inside the navigation system
  887.         UpdateNavigationSystemUsersForSynchronousWritingOperations();
  888.  
  889.         // step 3: update all the tasks that needs to read only the NavigationSystem data
  890.         // Those tasks may or may not be asynchronous tasks and the reading of the navigation data
  891.         // may extend in time after this function is actually executed.
  892.         UpdateNavigationSystemUsersForSynchronousOrAsynchronousReadingOperations();
  893.  
  894.         return m_state;
  895. }
  896.  
  897. void NavigationSystem::PauseNavigationUpdate()
  898. {
  899.         m_isNavigationUpdatePaused = true;
  900.         m_pEditorBackgroundUpdate->Pause(true);
  901. }
  902.  
  903. void NavigationSystem::RestartNavigationUpdate()
  904. {
  905.         m_isNavigationUpdatePaused = false;
  906.         m_pEditorBackgroundUpdate->Pause(false);
  907. }
  908.  
  909. #if NAVIGATION_SYSTEM_PC_ONLY
  910. void NavigationSystem::UpdateMeshes(const float frameTime, const bool blocking, const bool multiThreaded, const bool bBackground)
  911. {
  912.         if (m_isNavigationUpdatePaused || frameTime == .0f)
  913.                 return;
  914.  
  915.         m_debugDraw.UpdateWorkingProgress(frameTime, m_tileQueue.size());
  916.  
  917.         if (m_tileQueue.empty() && m_runningTasks.empty())
  918.         {
  919.                 if (m_state != Idle)
  920.                 {
  921.                         // We just finished the processing of the tiles, so before being in Idle
  922.                         // we need to recompute the Islands detection
  923.                         ComputeIslands();
  924.                 }
  925.                 m_state = Idle;
  926.                 return;
  927.         }
  928.  
  929.         size_t completed = 0;
  930.         size_t cacheHit = 0;
  931.  
  932.         while (true)
  933.         {
  934.                 if (!m_runningTasks.empty())
  935.                 {
  936.                         FRAME_PROFILER("Navigation System::UpdateMeshes() - Running Task Processing", gEnv->pSystem, PROFILE_AI);
  937.  
  938.                         RunningTasks::iterator it = m_runningTasks.begin();
  939.                         RunningTasks::iterator end = m_runningTasks.end();
  940.  
  941.                         for (; it != end; )
  942.                         {
  943.                                 const size_t resultSlot = *it;
  944.                                 assert(resultSlot < m_results.size());
  945.  
  946.                                 TileTaskResult& result = m_results[resultSlot];
  947.                                 if (result.state == TileTaskResult::Running)
  948.                                 {
  949.                                         ++it;
  950.                                         continue;
  951.                                 }
  952.  
  953.                                 CommitTile(result);
  954.  
  955.                                 {
  956.                                         FRAME_PROFILER("Navigation System::UpdateMeshes() - Running Task Processing - WaitForJob", gEnv->pSystem, PROFILE_AI);
  957.  
  958.                                         gEnv->GetJobManager()->WaitForJob(result.jobState);
  959.                                 }
  960.  
  961.                                 ++completed;
  962.                                 cacheHit += result.state == TileTaskResult::NoChanges;
  963.  
  964.                                 MNM::STile().Swap(result.tile);
  965.  
  966.                                 VolumeDefCopy& def = m_volumeDefCopy[result.volumeCopy];
  967.                                 --def.refCount;
  968.  
  969.                                 result.state = TileTaskResult::Running;
  970.                                 result.next = m_free;
  971.  
  972.                                 m_free = static_cast<uint16>(resultSlot);
  973.  
  974.                                 const bool reachedLastTask = (it == m_runningTasks.end() - 1);
  975.  
  976.                                 std::swap(*it, m_runningTasks.back());
  977.                                 m_runningTasks.pop_back();
  978.  
  979.                                 if (reachedLastTask)
  980.                                         break;  // prevent the invalidated iterator "it" from getting compared in the for-loop
  981.  
  982.                                 end = m_runningTasks.end();
  983.                         }
  984.                 }
  985.  
  986.                 m_throughput = completed / frameTime;
  987.                 m_cacheHitRate = cacheHit / frameTime;
  988.  
  989.                 if (m_tileQueue.empty() && m_runningTasks.empty())
  990.                 {
  991.                         if (m_state != Idle)
  992.                         {
  993.                                 // We just finished the processing of the tiles, so before being in Idle
  994.                                 // we need to recompute the Islands detection
  995.                                 ComputeIslands();
  996.                         }
  997.  
  998.                         m_state = Idle;
  999.  
  1000.                         return;
  1001.                 }
  1002.  
  1003.                 if (!m_tileQueue.empty())
  1004.                 {
  1005.                         m_state = Working;
  1006.  
  1007.                         FRAME_PROFILER("Navigation System::UpdateMeshes() - Job Spawning", gEnv->pSystem, PROFILE_AI);
  1008.                         const size_t idealMinimumTaskCount = 2;
  1009.                         const size_t MaxRunningTaskCount = multiThreaded ? m_maxRunningTaskCount : std::min(m_maxRunningTaskCount, idealMinimumTaskCount);
  1010.  
  1011.                         while (!m_tileQueue.empty() && (m_runningTasks.size() < MaxRunningTaskCount))
  1012.                         {
  1013.                                 const TileTask& task = m_tileQueue.front();
  1014.  
  1015.                                 if (task.aborted)
  1016.                                 {
  1017.                                         m_tileQueue.pop_front();
  1018.                                         continue;
  1019.                                 }
  1020.  
  1021.                                 const NavigationMesh& mesh = m_meshes[task.meshID];
  1022.                                 const MNM::CNavMesh::SGridParams& paramsGrid = mesh.navMesh.GetGridParams();
  1023.  
  1024.                                 m_runningTasks.push_back(m_free);
  1025.                                 CRY_ASSERT_MESSAGE(m_free < m_results.size(), "Index out of array bounds!");
  1026.                                 TileTaskResult& result = m_results[m_free];
  1027.                                 m_free = result.next;
  1028.  
  1029.                                 if (!SpawnJob(result, task.meshID, paramsGrid, task.x, task.y, task.z, multiThreaded))
  1030.                                 {
  1031.                                         result.state = TileTaskResult::Running;
  1032.                                         result.next = m_free;
  1033.  
  1034.                                         m_free = m_runningTasks.back();
  1035.                                         m_runningTasks.pop_back();
  1036.                                         break;
  1037.                                 }
  1038.  
  1039.                                 m_tileQueue.pop_front();
  1040.                         }
  1041.  
  1042.                         // keep main thread busy too if we're blocking
  1043.                         if (blocking && !m_tileQueue.empty() && multiThreaded)
  1044.                         {
  1045.                                 while (!m_tileQueue.empty())
  1046.                                 {
  1047.                                         const TileTask& task = m_tileQueue.front();
  1048.  
  1049.                                         if (task.aborted)
  1050.                                         {
  1051.                                                 m_tileQueue.pop_front();
  1052.                                                 continue;
  1053.                                         }
  1054.  
  1055.                                         NavigationMesh& mesh = m_meshes[task.meshID];
  1056.                                         const MNM::CNavMesh::SGridParams& paramsGrid = mesh.navMesh.GetGridParams();
  1057.  
  1058.                                         TileTaskResult result;
  1059.                                         if (!SpawnJob(result, task.meshID, paramsGrid, task.x, task.y, task.z, false))
  1060.                                                 break;
  1061.  
  1062.                                         CommitTile(result);
  1063.  
  1064.                                         m_tileQueue.pop_front();
  1065.                                         break;
  1066.                                 }
  1067.                         }
  1068.                 }
  1069.  
  1070.                 if (blocking && (!m_tileQueue.empty() || !m_runningTasks.empty()))
  1071.                         continue;
  1072.  
  1073.                 m_state = m_runningTasks.empty() ? Idle : Working;
  1074.  
  1075.                 return;
  1076.         }
  1077.  
  1078. }
  1079.  
  1080. void NavigationSystem::SetupGenerator(NavigationMeshID meshID, const MNM::CNavMesh::SGridParams& paramsGrid,
  1081.                                       uint16 x, uint16 y, uint16 z, MNM::CTileGenerator::Params& params,
  1082.                                       const MNM::BoundingVolume* boundary, const MNM::BoundingVolume* exclusions,
  1083.                                       size_t exclusionCount)
  1084. {
  1085.         const NavigationMesh& mesh = m_meshes[meshID];
  1086.  
  1087.         params.origin = paramsGrid.origin + Vec3i(x * paramsGrid.tileSize.x, y * paramsGrid.tileSize.y, z * paramsGrid.tileSize.z);
  1088.         params.voxelSize = paramsGrid.voxelSize;
  1089.         params.sizeX = paramsGrid.tileSize.x;
  1090.         params.sizeY = paramsGrid.tileSize.y;
  1091.         params.sizeZ = paramsGrid.tileSize.z;
  1092.         params.boundary = boundary;
  1093.         params.exclusions = exclusions;
  1094.         params.exclusionCount = static_cast<uint16>(exclusionCount);
  1095.  
  1096.         const AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  1097.  
  1098.         params.agent.radius = agentType.settings.radiusVoxelCount;
  1099.         params.agent.height = agentType.settings.heightVoxelCount;
  1100.         params.agent.climbableHeight = agentType.settings.climbableVoxelCount;
  1101.         params.agent.maxWaterDepth = agentType.settings.maxWaterDepthVoxelCount;
  1102.         params.climbableInclineGradient = agentType.settings.climbableInclineGradient;
  1103.         params.climbableStepRatio = agentType.settings.climbableStepRatio;
  1104.         params.agent.callback = agentType.meshEntityCallback;
  1105.  
  1106.         if (MNM::TileID tileID = mesh.navMesh.GetTileID(x, y, z))
  1107.                 params.hashValue = mesh.navMesh.GetTile(tileID).GetHashValue();
  1108.         else
  1109.                 params.flags |= MNM::CTileGenerator::Params::NoHashTest;
  1110.  
  1111.         params.pTileGeneratorExtensions = &m_tileGeneratorExtensionsContainer;
  1112.         params.navAgentTypeId = mesh.agentTypeID;
  1113. }
  1114.  
  1115. bool NavigationSystem::SpawnJob(TileTaskResult& result, NavigationMeshID meshID, const MNM::CNavMesh::SGridParams& paramsGrid,
  1116.                                 uint16 x, uint16 y, uint16 z, bool mt)
  1117. {
  1118.         result.x = x;
  1119.         result.y = y;
  1120.         result.z = z;
  1121.  
  1122.         result.meshID = meshID;
  1123.         result.state = TileTaskResult::Running;
  1124.         result.hashValue = 0;
  1125.  
  1126.         const NavigationMesh& mesh = m_meshes[meshID];
  1127.  
  1128.         size_t firstFree = ~0ul;
  1129.         size_t index = ~0ul;
  1130.  
  1131.         for (size_t i = 0; i < MaxVolumeDefCopyCount; ++i)
  1132.         {
  1133.                 VolumeDefCopy& def = m_volumeDefCopy[i];
  1134.                 if ((def.meshID == meshID) && (def.version == mesh.version))
  1135.                 {
  1136.                         index = i;
  1137.                         break;
  1138.                 }
  1139.                 else if ((firstFree == ~0ul) && !def.refCount)
  1140.                         firstFree = i;
  1141.         }
  1142.  
  1143.         if ((index == ~0ul) && (firstFree == ~0ul))
  1144.                 return false;
  1145.  
  1146.         VolumeDefCopy* def = 0;
  1147.  
  1148.         if (index != ~0ul)
  1149.                 def = &m_volumeDefCopy[index];
  1150.         else
  1151.         {
  1152.                 index = firstFree;
  1153.  
  1154.                 if (!m_volumes.validate(mesh.boundary))
  1155.                 {
  1156.                         AIWarning("NavigationSystem::SpawnJob: Detected non-valid mesh boundary volume (%u) for mesh %u '%s', skipping", (unsigned int)mesh.boundary, (unsigned int)meshID, mesh.name.c_str());
  1157.                         return false;
  1158.                 }
  1159.  
  1160.                 def = &m_volumeDefCopy[index];
  1161.                 def->meshID = meshID;
  1162.                 def->version = mesh.version;
  1163.                 def->boundary = m_volumes[mesh.boundary];
  1164.                 def->exclusions.clear();
  1165.                 def->exclusions.reserve(mesh.exclusions.size());
  1166.  
  1167.                 NavigationMesh::ExclusionVolumes::const_iterator eit = mesh.exclusions.begin();
  1168.                 NavigationMesh::ExclusionVolumes::const_iterator eend = mesh.exclusions.end();
  1169.  
  1170.                 for (; eit != eend; ++eit)
  1171.                 {
  1172.                         const NavigationVolumeID exclusionVolumeID = *eit;
  1173.                         assert(m_volumes.validate(exclusionVolumeID));
  1174.  
  1175.                         if (m_volumes.validate(exclusionVolumeID))
  1176.                         {
  1177.                                 def->exclusions.push_back(m_volumes[exclusionVolumeID]);
  1178.                         }
  1179.                         else
  1180.                         {
  1181.                                 CryLogAlways("NavigationSystem::SpawnJob(): Detected non-valid exclusion volume (%d) for mesh '%s', skipping", (uint32)exclusionVolumeID, mesh.name.c_str());
  1182.                         }
  1183.                 }
  1184.         }
  1185.  
  1186.         result.volumeCopy = static_cast<uint16>(index);
  1187.  
  1188.         ++def->refCount;
  1189.  
  1190.         MNM::CTileGenerator::Params params;
  1191.  
  1192.         SetupGenerator(meshID, paramsGrid, x, y, z, params, &def->boundary,
  1193.                        def->exclusions.empty() ? 0 : &def->exclusions[0], def->exclusions.size());
  1194.  
  1195.         if (mt)
  1196.         {
  1197.                 NavigationGenerationJob job(params, &result.state, &result.tile, &result.hashValue);
  1198.                 job.RegisterJobState(&result.jobState);
  1199.                 job.SetPriorityLevel(JobManager::eStreamPriority);
  1200.                 job.Run();
  1201.         }
  1202.         else
  1203.         {
  1204.                 GenerateTileJob(params, &result.state, &result.tile, &result.hashValue);
  1205.         }
  1206.  
  1207.         if (gAIEnv.CVars.DebugDrawNavigation)
  1208.         {
  1209.                 if (gEnv->pRenderer)
  1210.                 {
  1211.                         if (IRenderAuxGeom* pRenderAuxGeom = gEnv->pRenderer->GetIRenderAuxGeom())
  1212.                         {
  1213.                                 pRenderAuxGeom->DrawAABB(AABB(params.origin, params.origin + Vec3((float)params.sizeX, (float)params.sizeY, (float)params.sizeZ)),
  1214.                                                          IDENTITY, false, Col_Red, eBBD_Faceted);
  1215.                         }
  1216.                 }
  1217.         }
  1218.  
  1219.         return true;
  1220. }
  1221.  
  1222. void NavigationSystem::CommitTile(TileTaskResult& result)
  1223. {
  1224.         // The mesh for this tile has been destroyed, it doesn't make sense to commit the tile
  1225.         const bool nonValidTile = (m_meshes.validate(result.meshID) == false);
  1226.         if (nonValidTile)
  1227.         {
  1228.                 return;
  1229.         }
  1230.  
  1231.         NavigationMesh& mesh = m_meshes[result.meshID];
  1232.  
  1233.         switch (result.state)
  1234.         {
  1235.         case TileTaskResult::Completed:
  1236.                 {
  1237.                         FRAME_PROFILER("Navigation System::CommitTile() - Running Task Processing - ConnectToNetwork", gEnv->pSystem, PROFILE_AI);
  1238.  
  1239.                         MNM::TileID tileID = mesh.navMesh.SetTile(result.x, result.y, result.z, result.tile);
  1240.                         mesh.navMesh.ConnectToNetwork(tileID);
  1241.  
  1242.                         m_offMeshNavigationManager.RefreshConnections(result.meshID, tileID);
  1243.                         gAIEnv.pMNMPathfinder->OnNavigationMeshChanged(result.meshID, tileID);
  1244.  
  1245.                         const AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  1246.                         AgentType::Callbacks::const_iterator cit = agentType.callbacks.begin();
  1247.                         AgentType::Callbacks::const_iterator cend = agentType.callbacks.end();
  1248.  
  1249.                         for (; cit != cend; ++cit)
  1250.                         {
  1251.                                 const NavigationMeshChangeCallback& callback = *cit;
  1252.                                 callback(mesh.agentTypeID, result.meshID, tileID);
  1253.                         }
  1254.                 }
  1255.                 break;
  1256.         case TileTaskResult::Failed:
  1257.                 {
  1258.                         FRAME_PROFILER("Navigation System::CommitTile() - Running Task Processing - ClearTile", gEnv->pSystem, PROFILE_AI);
  1259.  
  1260.                         if (MNM::TileID tileID = mesh.navMesh.GetTileID(result.x, result.y, result.z))
  1261.                         {
  1262.                                 mesh.navMesh.ClearTile(tileID);
  1263.  
  1264.                                 m_offMeshNavigationManager.RefreshConnections(result.meshID, tileID);
  1265.                                 gAIEnv.pMNMPathfinder->OnNavigationMeshChanged(result.meshID, tileID);
  1266.  
  1267.                                 const AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  1268.                                 AgentType::Callbacks::const_iterator cit = agentType.callbacks.begin();
  1269.                                 AgentType::Callbacks::const_iterator cend = agentType.callbacks.end();
  1270.  
  1271.                                 for (; cit != cend; ++cit)
  1272.                                 {
  1273.                                         const NavigationMeshChangeCallback& callback = *cit;
  1274.                                         callback(mesh.agentTypeID, result.meshID, tileID);
  1275.                                 }
  1276.                         }
  1277.                 }
  1278.                 break;
  1279.         case TileTaskResult::NoChanges:
  1280.                 break;
  1281.         default:
  1282.                 assert(0);
  1283.                 break;
  1284.         }
  1285. }
  1286. #endif
  1287.  
  1288. size_t NavigationSystem::QueueMeshUpdate(NavigationMeshID meshID, const AABB& aabb)
  1289. {
  1290.         assert(meshID != 0);
  1291.  
  1292.         size_t affectedCount = 0;
  1293. #if NAVIGATION_SYSTEM_PC_ONLY
  1294.         if (meshID && m_meshes.validate(meshID))
  1295.         {
  1296.                 FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1297.  
  1298.                 NavigationMesh& mesh = m_meshes[meshID];
  1299.                 MNM::CNavMesh& navMesh = mesh.navMesh;
  1300.  
  1301.                 const MNM::CNavMesh::SGridParams& paramsGrid = navMesh.GetGridParams();
  1302.  
  1303.                 if (aabb.IsEmpty() || !mesh.boundary)
  1304.                         return 0;
  1305.  
  1306.                 const AABB& boundary = m_volumes[mesh.boundary].aabb;
  1307.                 const AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  1308.  
  1309.                 const float extraH = std::max(paramsGrid.voxelSize.x, paramsGrid.voxelSize.y) * (agentType.settings.radiusVoxelCount + 1);
  1310.                 const float extraV = paramsGrid.voxelSize.z * (agentType.settings.heightVoxelCount + 1);
  1311.                 const float extraVM = paramsGrid.voxelSize.z; // tiles above are not directly influenced
  1312.  
  1313.                 Vec3 bmin(std::max(0.0f, std::max(boundary.min.x, aabb.min.x - extraH) - paramsGrid.origin.x),
  1314.                           std::max(0.0f, std::max(boundary.min.y, aabb.min.y - extraH) - paramsGrid.origin.y),
  1315.                           std::max(0.0f, std::max(boundary.min.z, aabb.min.z - extraV) - paramsGrid.origin.z));
  1316.  
  1317.                 Vec3 bmax(std::max(0.0f, std::min(boundary.max.x, aabb.max.x + extraH) - paramsGrid.origin.x),
  1318.                           std::max(0.0f, std::min(boundary.max.y, aabb.max.y + extraH) - paramsGrid.origin.y),
  1319.                           std::max(0.0f, std::min(boundary.max.z, aabb.max.z + extraVM) - paramsGrid.origin.z));
  1320.  
  1321.                 uint16 xmin = (uint16)(floor_tpl(bmin.x / (float)paramsGrid.tileSize.x));
  1322.                 uint16 xmax = (uint16)(floor_tpl(bmax.x / (float)paramsGrid.tileSize.x));
  1323.  
  1324.                 uint16 ymin = (uint16)(floor_tpl(bmin.y / (float)paramsGrid.tileSize.y));
  1325.                 uint16 ymax = (uint16)(floor_tpl(bmax.y / (float)paramsGrid.tileSize.y));
  1326.  
  1327.                 uint16 zmin = (uint16)(floor_tpl(bmin.z / (float)paramsGrid.tileSize.z));
  1328.                 uint16 zmax = (uint16)(floor_tpl(bmax.z / (float)paramsGrid.tileSize.z));
  1329.  
  1330.                 TileTaskQueue::iterator it = m_tileQueue.begin();
  1331.                 TileTaskQueue::iterator rear = m_tileQueue.end();
  1332.  
  1333.                 for (; it != rear; )
  1334.                 {
  1335.                         TileTask& task = *it;
  1336.  
  1337.                         if ((task.meshID == meshID) && (task.x >= xmin) && (task.x <= xmax) &&
  1338.                             (task.y >= ymin) && (task.y <= ymax) &&
  1339.                             (task.z >= zmin) && (task.z <= zmax))
  1340.                         {
  1341.                                 rear = rear - 1;
  1342.                                 std::swap(task, *rear);
  1343.  
  1344.                                 continue;
  1345.                         }
  1346.                         ++it;
  1347.                 }
  1348.  
  1349.                 if (rear != m_tileQueue.end())
  1350.                 {
  1351.                         m_tileQueue.erase(rear, m_tileQueue.end());
  1352.                 }
  1353.  
  1354.                 for (size_t y = ymin; y <= ymax; ++y)
  1355.                 {
  1356.                         for (size_t x = xmin; x <= xmax; ++x)
  1357.                         {
  1358.                                 for (size_t z = zmin; z <= zmax; ++z)
  1359.                                 {
  1360.                                         TileTask task;
  1361.                                         task.meshID = meshID;
  1362.                                         task.x = (uint16)x;
  1363.                                         task.y = (uint16)y;
  1364.                                         task.z = (uint16)z;
  1365.  
  1366.                                         m_tileQueue.push_back(task);
  1367.  
  1368.                                         ++affectedCount;
  1369.                                 }
  1370.                         }
  1371.                 }
  1372.         }
  1373.  
  1374. #endif
  1375.  
  1376.         return affectedCount;
  1377. }
  1378.  
  1379. void NavigationSystem::ProcessQueuedMeshUpdates()
  1380. {
  1381. #if NAVIGATION_SYSTEM_PC_ONLY
  1382.         do
  1383.         {
  1384.                 UpdateMeshes(0.0333f, false, gAIEnv.CVars.NavigationSystemMT != 0, false);
  1385.         }
  1386.         while (m_state == INavigationSystem::Working);
  1387. #endif
  1388. }
  1389.  
  1390. void NavigationSystem::QueueDifferenceUpdate(NavigationMeshID meshID, const NavigationBoundingVolume& oldVolume,
  1391.                                              const NavigationBoundingVolume& newVolume)
  1392. {
  1393. #if NAVIGATION_SYSTEM_PC_ONLY
  1394.         // TODO: implement properly by verifying what didn't change
  1395.         // since there will be loads of volume-aabb intersection tests,
  1396.         // this should be a new job running in a different thread
  1397.         // producing an array of all the tiles that need updating
  1398.         // which then gets concatenated into m_tileQueue in the main update
  1399.  
  1400.         if (meshID && m_meshes.validate(meshID))
  1401.         {
  1402.                 FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1403.  
  1404.                 NavigationMesh& mesh = m_meshes[meshID];
  1405.                 MNM::CNavMesh& navMesh = mesh.navMesh;
  1406.  
  1407.                 const MNM::CNavMesh::SGridParams& paramsGrid = navMesh.GetGridParams();
  1408.  
  1409.                 AABB aabb = oldVolume.aabb;
  1410.                 aabb.Add(newVolume.aabb);
  1411.  
  1412.                 const AgentType& agentType = m_agentTypes[mesh.agentTypeID - 1];
  1413.  
  1414.                 const float extraH = std::max(paramsGrid.voxelSize.x, paramsGrid.voxelSize.y) * (agentType.settings.radiusVoxelCount + 1);
  1415.                 const float extraV = paramsGrid.voxelSize.z * (agentType.settings.heightVoxelCount + 1);
  1416.                 const float extraVM = paramsGrid.voxelSize.z; // tiles above are not directly influenced
  1417.  
  1418.                 Vec3 bmin(std::max(0.0f, (aabb.min.x - extraH) - paramsGrid.origin.x),
  1419.                           std::max(0.0f, (aabb.min.y - extraH) - paramsGrid.origin.y),
  1420.                           std::max(0.0f, (aabb.min.z - extraV) - paramsGrid.origin.z));
  1421.  
  1422.                 Vec3 bmax(std::max(0.0f, (aabb.max.x + extraH) - paramsGrid.origin.x),
  1423.                           std::max(0.0f, (aabb.max.y + extraH) - paramsGrid.origin.y),
  1424.                           std::max(0.0f, (aabb.max.z + extraVM) - paramsGrid.origin.z));
  1425.  
  1426.                 uint16 xmin = (uint16)(floor_tpl(bmin.x / (float)paramsGrid.tileSize.x));
  1427.                 uint16 xmax = (uint16)(floor_tpl(bmax.x / (float)paramsGrid.tileSize.x));
  1428.  
  1429.                 uint16 ymin = (uint16)(floor_tpl(bmin.y / (float)paramsGrid.tileSize.y));
  1430.                 uint16 ymax = (uint16)(floor_tpl(bmax.y / (float)paramsGrid.tileSize.y));
  1431.  
  1432.                 uint16 zmin = (uint16)(floor_tpl(bmin.z / (float)paramsGrid.tileSize.z));
  1433.                 uint16 zmax = (uint16)(floor_tpl(bmax.z / (float)paramsGrid.tileSize.z));
  1434.  
  1435.                 TileTaskQueue::iterator it = m_tileQueue.begin();
  1436.                 TileTaskQueue::iterator rear = m_tileQueue.end();
  1437.  
  1438.                 for (; it != rear; )
  1439.                 {
  1440.                         TileTask& task = *it;
  1441.  
  1442.                         if ((task.meshID == meshID) && (task.x >= xmin) && (task.x <= xmax) &&
  1443.                             (task.y >= ymin) && (task.y <= ymax) &&
  1444.                             (task.z >= zmin) && (task.z <= zmax))
  1445.                         {
  1446.                                 rear = rear - 1;
  1447.                                 std::swap(task, *rear);
  1448.  
  1449.                                 continue;
  1450.                         }
  1451.                         ++it;
  1452.                 }
  1453.  
  1454.                 if (rear != m_tileQueue.end())
  1455.                 {
  1456.                         m_tileQueue.erase(rear, m_tileQueue.end());
  1457.                 }
  1458.  
  1459.                 for (size_t y = ymin; y <= ymax; ++y)
  1460.                 {
  1461.                         for (size_t x = xmin; x <= xmax; ++x)
  1462.                         {
  1463.                                 for (size_t z = zmin; z <= zmax; ++z)
  1464.                                 {
  1465.                                         TileTask task;
  1466.                                         task.meshID = meshID;
  1467.                                         task.x = (uint16)x;
  1468.                                         task.y = (uint16)y;
  1469.                                         task.z = (uint16)z;
  1470.  
  1471.                                         m_tileQueue.push_back(task);
  1472.                                 }
  1473.                         }
  1474.                 }
  1475.         }
  1476. #endif
  1477. }
  1478.  
  1479. void NavigationSystem::WorldChanged(const AABB& aabb)
  1480. {
  1481. #if NAVIGATION_SYSTEM_PC_ONLY
  1482.         if (!aabb.IsEmpty() && Overlap::AABB_AABB(m_worldAABB, aabb))
  1483.         {
  1484.                 AgentTypes::const_iterator it = m_agentTypes.begin();
  1485.                 AgentTypes::const_iterator end = m_agentTypes.end();
  1486.  
  1487.                 for (; it != end; ++it)
  1488.                 {
  1489.                         const AgentType& agentType = *it;
  1490.  
  1491.                         AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  1492.                         AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  1493.  
  1494.                         for (; mit != mend; ++mit)
  1495.                         {
  1496.                                 const NavigationMeshID meshID = mit->id;
  1497.                                 const NavigationMesh& mesh = m_meshes[meshID];
  1498.  
  1499.                                 if (mesh.boundary && Overlap::AABB_AABB(aabb, m_volumes[mesh.boundary].aabb))
  1500.                                         QueueMeshUpdate(meshID, aabb);
  1501.                         }
  1502.                 }
  1503.         }
  1504. #endif
  1505. }
  1506.  
  1507. void NavigationSystem::StopAllTasks()
  1508. {
  1509.         for (size_t t = 0; t < m_runningTasks.size(); ++t)
  1510.                 m_results[m_runningTasks[t]].state = TileTaskResult::Failed;
  1511.  
  1512.         for (size_t t = 0; t < m_runningTasks.size(); ++t)
  1513.                 gEnv->pJobManager->WaitForJob(m_results[m_runningTasks[t]].jobState);
  1514.  
  1515.         for (size_t t = 0; t < m_runningTasks.size(); ++t)
  1516.                 m_results[m_runningTasks[t]].tile.Destroy();
  1517.  
  1518.         m_runningTasks.clear();
  1519. }
  1520.  
  1521. void NavigationSystem::ComputeIslands()
  1522. {
  1523.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1524.  
  1525.         m_islandConnectionsManager.Reset();
  1526.  
  1527.         AgentTypes::const_iterator it = m_agentTypes.begin();
  1528.         AgentTypes::const_iterator end = m_agentTypes.end();
  1529.  
  1530.         for (; it != end; ++it)
  1531.         {
  1532.                 const AgentType& agentType = *it;
  1533.                 AgentType::Meshes::const_iterator itMesh = agentType.meshes.begin();
  1534.                 AgentType::Meshes::const_iterator endMesh = agentType.meshes.end();
  1535.                 for (; itMesh != endMesh; ++itMesh)
  1536.                 {
  1537.                         if (itMesh->id && m_meshes.validate(itMesh->id))
  1538.                         {
  1539.                                 NavigationMeshID meshID = itMesh->id;
  1540.                                 MNM::IslandConnections& islandConnections = m_islandConnectionsManager.GetIslandConnections();
  1541.                                 NavigationMesh& mesh = m_meshes[meshID];
  1542.                                 mesh.navMesh.ComputeStaticIslandsAndConnections(meshID, m_offMeshNavigationManager, islandConnections);
  1543.                         }
  1544.                 }
  1545.         }
  1546. }
  1547.  
  1548. void NavigationSystem::AddIslandConnectionsBetweenTriangles(const NavigationMeshID& meshID, const MNM::TriangleID startingTriangleID,
  1549.                                                             const MNM::TriangleID endingTriangleID)
  1550. {
  1551.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1552.  
  1553.         if (m_meshes.validate(meshID))
  1554.         {
  1555.                 NavigationMesh& mesh = m_meshes[meshID];
  1556.                 MNM::Tile::STriangle startingTriangle, endingTriangle;
  1557.                 if (mesh.navMesh.GetTriangle(startingTriangleID, startingTriangle))
  1558.                 {
  1559.                         if (mesh.navMesh.GetTriangle(endingTriangleID, endingTriangle))
  1560.                         {
  1561.                                 MNM::GlobalIslandID startingIslandID(meshID, startingTriangle.islandID);
  1562.  
  1563.                                 MNM::STile& tile = mesh.navMesh.GetTile(MNM::ComputeTileID(startingTriangleID));
  1564.                                 for (uint16 l = 0; l < startingTriangle.linkCount; ++l)
  1565.                                 {
  1566.                                         const MNM::Tile::SLink& link = tile.GetLinks()[startingTriangle.firstLink + l];
  1567.                                         if (link.side == MNM::Tile::SLink::OffMesh)
  1568.                                         {
  1569. #if DEBUG_MNM_LOG_OFFMESH_LINK_OPERATIONS
  1570.                                                 AILogCommentID("<MNM:OffMeshLink>", "NavigationSystem::AddIslandConnectionsBetweenTriangles link from %u to %u (mesh %u)", startingTriangle.islandID, endingTriangle.islandID, meshID);
  1571. #endif
  1572.                                                 MNM::GlobalIslandID endingIslandID(meshID, endingTriangle.islandID);
  1573.                                                 MNM::OffMeshNavigation& offMeshNavigation = m_offMeshNavigationManager.GetOffMeshNavigationForMesh(meshID);
  1574.                                                 MNM::OffMeshNavigation::QueryLinksResult linksResult = offMeshNavigation.GetLinksForTriangle(startingTriangleID, link.triangle);
  1575.                                                 while (MNM::WayTriangleData nextTri = linksResult.GetNextTriangle())
  1576.                                                 {
  1577.                                                         if (nextTri.triangleID == endingTriangleID)
  1578.                                                         {
  1579.                                                                 const MNM::OffMeshLink* pLink = m_offMeshNavigationManager.GetOffMeshLink(nextTri.offMeshLinkID);
  1580.                                                                 assert(pLink);
  1581.                                                                 MNM::IslandConnections::Link islandLink(nextTri.triangleID, nextTri.offMeshLinkID, endingIslandID, pLink->GetEntityIdForOffMeshLink());
  1582.                                                                 MNM::IslandConnections& islandConnections = m_islandConnectionsManager.GetIslandConnections();
  1583.                                                                 islandConnections.SetOneWayConnectionBetweenIsland(startingIslandID, islandLink);
  1584.                                                         }
  1585.                                                 }
  1586.                                         }
  1587.                                 }
  1588.                         }
  1589.                 }
  1590.         }
  1591. }
  1592.  
  1593. void NavigationSystem::RemoveAllIslandConnectionsForObject(const NavigationMeshID& meshID, const uint32 objectId)
  1594. {
  1595.         MNM::IslandConnections& islandConnections = m_islandConnectionsManager.GetIslandConnections();
  1596.         islandConnections.RemoveAllIslandConnectionsForObject(meshID, objectId);
  1597. }
  1598.  
  1599. void NavigationSystem::RemoveIslandsConnectionBetweenTriangles(const NavigationMeshID& meshID, const MNM::TriangleID startingTriangleID,
  1600.                                                                const MNM::TriangleID endingTriangleID)
  1601. {
  1602.         // NOTE pavloi 2016.02.05: be advised, that this function is not use anywhere. It should be called before triangles are unlinked
  1603.         // from each other, but currently OffMeshNavigationManager first unlinks triangles and only then unlinks islands.
  1604.  
  1605.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1606.  
  1607.         if (m_meshes.validate(meshID))
  1608.         {
  1609.                 NavigationMesh& mesh = m_meshes[meshID];
  1610.                 MNM::Tile::STriangle startingTriangle, endingTriangle;
  1611.                 if (mesh.navMesh.GetTriangle(startingTriangleID, startingTriangle))
  1612.                 {
  1613.                         if (mesh.navMesh.GetTriangle(endingTriangleID, endingTriangle))
  1614.                         {
  1615.                                 MNM::GlobalIslandID startingIslandID(meshID, startingTriangle.islandID);
  1616.  
  1617.                                 MNM::STile& tile = mesh.navMesh.GetTile(MNM::ComputeTileID(startingTriangleID));
  1618.                                 for (uint16 l = 0; l < startingTriangle.linkCount; ++l)
  1619.                                 {
  1620.                                         const MNM::Tile::SLink& link = tile.GetLinks()[startingTriangle.firstLink + l];
  1621.                                         if (link.side == MNM::Tile::SLink::OffMesh)
  1622.                                         {
  1623. #if DEBUG_MNM_LOG_OFFMESH_LINK_OPERATIONS
  1624.                                                 AILogCommentID("<MNM:OffMeshLink>", "NavigationSystem::RemoveIslandsConnectionBetweenTriangles link from %u to %u (mesh %u)", startingTriangle.islandID, endingTriangle.islandID, meshID);
  1625. #endif
  1626.                                                 MNM::GlobalIslandID endingIslandID(meshID, endingTriangle.islandID);
  1627.                                                 MNM::OffMeshNavigation& offMeshNavigation = m_offMeshNavigationManager.GetOffMeshNavigationForMesh(meshID);
  1628.                                                 MNM::OffMeshNavigation::QueryLinksResult linksResult = offMeshNavigation.GetLinksForTriangle(startingTriangleID, link.triangle);
  1629.                                                 while (MNM::WayTriangleData nextTri = linksResult.GetNextTriangle())
  1630.                                                 {
  1631.                                                         if (nextTri.triangleID == endingTriangleID)
  1632.                                                         {
  1633.                                                                 const MNM::OffMeshLink* pLink = m_offMeshNavigationManager.GetOffMeshLink(nextTri.offMeshLinkID);
  1634.                                                                 assert(pLink);
  1635.                                                                 MNM::IslandConnections::Link islandLink(nextTri.triangleID, nextTri.offMeshLinkID, endingIslandID, pLink->GetEntityIdForOffMeshLink());
  1636.                                                                 MNM::IslandConnections& islandConnections = m_islandConnectionsManager.GetIslandConnections();
  1637.                                                                 islandConnections.RemoveOneWayConnectionBetweenIsland(startingIslandID, islandLink);
  1638.                                                         }
  1639.                                                 }
  1640.                                         }
  1641.                                 }
  1642.                         }
  1643.                 }
  1644.         }
  1645. }
  1646.  
  1647. void NavigationSystem::AddOffMeshLinkIslandConnectionsBetweenTriangles(
  1648.   const NavigationMeshID& meshID,
  1649.   const MNM::TriangleID startingTriangleID,
  1650.   const MNM::TriangleID endingTriangleID,
  1651.   const MNM::OffMeshLinkID& linkID)
  1652. {
  1653.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1654.  
  1655. #if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
  1656.         {
  1657.                 bool bLinkIsFound = false;
  1658.                 // Next piece code is an almost exact copy from AddIslandConnectionsBetweenTriangles()
  1659.                 if (m_meshes.validate(meshID))
  1660.                 {
  1661.                         const NavigationMesh& mesh = m_meshes[meshID];
  1662.                         MNM::Tile::STriangle startingTriangle, endingTriangle;
  1663.                         if (mesh.navMesh.GetTriangle(startingTriangleID, startingTriangle))
  1664.                         {
  1665.                                 if (mesh.navMesh.GetTriangle(endingTriangleID, endingTriangle))
  1666.                                 {
  1667.                                         const MNM::GlobalIslandID startingIslandID(meshID, startingTriangle.islandID);
  1668.                                         const MNM::STile& tile = mesh.navMesh.GetTile(MNM::ComputeTileID(startingTriangleID));
  1669.                                         for (uint16 l = 0; l < startingTriangle.linkCount && !bLinkIsFound; ++l)
  1670.                                         {
  1671.                                                 const MNM::Tile::SLink& link = tile.GetLinks()[startingTriangle.firstLink + l];
  1672.                                                 if (link.side == MNM::Tile::SLink::OffMesh)
  1673.                                                 {
  1674.                                                         const MNM::GlobalIslandID endingIslandID(meshID, endingTriangle.islandID);
  1675.                                                         const MNM::OffMeshNavigation& offMeshNavigation = m_offMeshNavigationManager.GetOffMeshNavigationForMesh(meshID);
  1676.                                                         const MNM::OffMeshNavigation::QueryLinksResult linksResult = offMeshNavigation.GetLinksForTriangle(startingTriangleID, link.triangle);
  1677.                                                         while (MNM::WayTriangleData nextTri = linksResult.GetNextTriangle())
  1678.                                                         {
  1679.                                                                 if (nextTri.triangleID == endingTriangleID)
  1680.                                                                 {
  1681.                                                                         if (nextTri.offMeshLinkID == linkID)
  1682.                                                                         {
  1683.                                                                                 if (const MNM::OffMeshLink* pLink = m_offMeshNavigationManager.GetOffMeshLink(nextTri.offMeshLinkID))
  1684.                                                                                 {
  1685.                                                                                         bLinkIsFound = true;
  1686.                                                                                         break;
  1687.                                                                                 }
  1688.                                                                         }
  1689.                                                                 }
  1690.                                                         }
  1691.                                                 }
  1692.                                         }
  1693.                                 }
  1694.                         }
  1695.                 }
  1696.  
  1697.                 if (!bLinkIsFound)
  1698.                 {
  1699.                         // It's expected, that the triangles in tiles are already linked together before this function is called.
  1700.                         // But we haven't found a link during validation.
  1701.                         AIErrorID("<MNM:OffMeshLink>", "NavigationSystem::AddOffMeshLinkIslandConnectionsBetweenTriangles is called with wrong input");
  1702.                 }
  1703.         }
  1704. #endif // DEBUG_MNM_DATA_CONSISTENCY_ENABLED
  1705.  
  1706.         // TODO pavloi 2016.02.05: whole piece is suboptimal - this function is called from m_offMeshNavigationManager already, where
  1707.         // it linked triangles and had full info about them. I leave it like this to be consistent with AddIslandConnectionsBetweenTriangles()
  1708.         if (m_meshes.validate(meshID))
  1709.         {
  1710.                 NavigationMesh& mesh = m_meshes[meshID];
  1711.                 MNM::Tile::STriangle startingTriangle, endingTriangle;
  1712.                 if (mesh.navMesh.GetTriangle(startingTriangleID, startingTriangle))
  1713.                 {
  1714.                         if (mesh.navMesh.GetTriangle(endingTriangleID, endingTriangle))
  1715.                         {
  1716.                                 const MNM::GlobalIslandID startingIslandID(meshID, startingTriangle.islandID);
  1717.                                 const MNM::GlobalIslandID endingIslandID(meshID, endingTriangle.islandID);
  1718.  
  1719.                                 const MNM::OffMeshLink* pLink = m_offMeshNavigationManager.GetOffMeshLink(linkID);
  1720.                                 if (pLink)
  1721.                                 {
  1722.                                         const MNM::IslandConnections::Link islandLink(endingTriangleID, linkID, endingIslandID, pLink->GetEntityIdForOffMeshLink());
  1723.                                         MNM::IslandConnections& islandConnections = m_islandConnectionsManager.GetIslandConnections();
  1724.                                         islandConnections.SetOneWayConnectionBetweenIsland(startingIslandID, islandLink);
  1725.                                 }
  1726.                         }
  1727.                 }
  1728.         }
  1729. }
  1730.  
  1731. void NavigationSystem::RemoveOffMeshLinkIslandsConnectionBetweenTriangles(
  1732.   const NavigationMeshID& meshID,
  1733.   const MNM::TriangleID startingTriangleID,
  1734.   const MNM::TriangleID endingTriangleID,
  1735.   const MNM::OffMeshLinkID& linkID)
  1736. {
  1737.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1738. #if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
  1739.         {
  1740.                 bool bLinkIsFound = false;
  1741.                 // Next piece code is an almost exact copy from RemoveIslandConnectionsBetweenTriangles()
  1742.                 if (m_meshes.validate(meshID))
  1743.                 {
  1744.                         const NavigationMesh& mesh = m_meshes[meshID];
  1745.                         MNM::Tile::STriangle startingTriangle, endingTriangle;
  1746.                         if (mesh.navMesh.GetTriangle(startingTriangleID, startingTriangle))
  1747.                         {
  1748.                                 if (mesh.navMesh.GetTriangle(endingTriangleID, endingTriangle))
  1749.                                 {
  1750.                                         const MNM::GlobalIslandID startingIslandID(meshID, startingTriangle.islandID);
  1751.  
  1752.                                         const MNM::STile& tile = mesh.navMesh.GetTile(MNM::ComputeTileID(startingTriangleID));
  1753.                                         for (uint16 l = 0; l < startingTriangle.linkCount && !bLinkIsFound; ++l)
  1754.                                         {
  1755.                                                 const MNM::Tile::SLink& link = tile.GetLinks()[startingTriangle.firstLink + l];
  1756.                                                 if (link.side == MNM::Tile::SLink::OffMesh)
  1757.                                                 {
  1758.                                                         const MNM::GlobalIslandID endingIslandID(meshID, endingTriangle.islandID);
  1759.                                                         const MNM::OffMeshNavigation& offMeshNavigation = m_offMeshNavigationManager.GetOffMeshNavigationForMesh(meshID);
  1760.                                                         const MNM::OffMeshNavigation::QueryLinksResult linksResult = offMeshNavigation.GetLinksForTriangle(startingTriangleID, link.triangle);
  1761.                                                         while (MNM::WayTriangleData nextTri = linksResult.GetNextTriangle())
  1762.                                                         {
  1763.                                                                 if (nextTri.triangleID == endingTriangleID)
  1764.                                                                 {
  1765.                                                                         if (nextTri.offMeshLinkID == linkID)
  1766.                                                                         {
  1767.                                                                                 if (const MNM::OffMeshLink* pLink = m_offMeshNavigationManager.GetOffMeshLink(nextTri.offMeshLinkID))
  1768.                                                                                 {
  1769.                                                                                         bLinkIsFound = true;
  1770.                                                                                         break;
  1771.                                                                                 }
  1772.                                                                         }
  1773.                                                                 }
  1774.                                                         }
  1775.                                                 }
  1776.                                         }
  1777.                                 }
  1778.                         }
  1779.                 }
  1780.  
  1781.                 if (bLinkIsFound)
  1782.                 {
  1783.                         // It's expected, that the triangles in tiles are already unlinked from each other before this function is called.
  1784.                         // But validation actually found, that the link is still there.
  1785.                         AIErrorID("<MNM:OffMeshLink>", "NavigationSystem::RemoveOffMeshLinkIslandsConnectionBetweenTriangles is called with wrong input");
  1786.                 }
  1787.         }
  1788. #endif // DEBUG_MNM_DATA_CONSISTENCY_ENABLED
  1789.  
  1790.         // TODO pavloi 2016.02.05: whole piece is suboptimal - this function is called from m_offMeshNavigationManager already, where
  1791.         // it unlinked triangles and had full info about them. I leave it like this to be consistent with RemoveIslandConnectionsBetweenTriangles()
  1792.         if (m_meshes.validate(meshID))
  1793.         {
  1794.                 NavigationMesh& mesh = m_meshes[meshID];
  1795.                 MNM::Tile::STriangle startingTriangle, endingTriangle;
  1796.                 if (mesh.navMesh.GetTriangle(startingTriangleID, startingTriangle))
  1797.                 {
  1798.                         if (mesh.navMesh.GetTriangle(endingTriangleID, endingTriangle))
  1799.                         {
  1800.                                 const MNM::GlobalIslandID startingIslandID(meshID, startingTriangle.islandID);
  1801.                                 const MNM::GlobalIslandID endingIslandID(meshID, endingTriangle.islandID);
  1802.  
  1803.                                 const MNM::OffMeshLink* pLink = m_offMeshNavigationManager.GetOffMeshLink(linkID);
  1804.                                 if (pLink)
  1805.                                 {
  1806.                                         const MNM::IslandConnections::Link islandLink(endingTriangleID, linkID, endingIslandID, pLink->GetEntityIdForOffMeshLink());
  1807.                                         MNM::IslandConnections& islandConnections = m_islandConnectionsManager.GetIslandConnections();
  1808.                                         islandConnections.RemoveOneWayConnectionBetweenIsland(startingIslandID, islandLink);
  1809.                                 }
  1810.                         }
  1811.                 }
  1812.         }
  1813. }
  1814.  
  1815. #if MNM_USE_EXPORT_INFORMATION
  1816. void NavigationSystem::ComputeAccessibility(IAIObject* pIAIObject, NavigationAgentTypeID agentTypeId /* = NavigationAgentTypeID(0) */)
  1817. {
  1818.         const CAIActor* actor = CastToCAIActorSafe(pIAIObject);
  1819.         const Vec3 debugLocation = pIAIObject->GetEntity()->GetPos(); // we're using the IEntity's position (not the IAIObject one's), because the CAIObject one's is always some time behind due to the way its private position is queried via BodyInfo -> StanceState -> eye position
  1820.         const NavigationAgentTypeID actorTypeId = actor ? actor->GetNavigationTypeID() : NavigationAgentTypeID(0);
  1821.         const NavigationAgentTypeID agentTypeIdForAccessibilityCalculation = agentTypeId ? agentTypeId : actorTypeId;
  1822.         NavigationMeshID meshId = GetEnclosingMeshID(agentTypeIdForAccessibilityCalculation, debugLocation);
  1823.  
  1824.         if (meshId)
  1825.         {
  1826.                 NavigationMesh& mesh = GetMesh(meshId);
  1827.                 const MNM::CNavMesh::SGridParams& paramsGrid = mesh.navMesh.GetGridParams();
  1828.                 const MNM::OffMeshNavigation& offMeshNavigation = GetOffMeshNavigationManager()->GetOffMeshNavigationForMesh(meshId);
  1829.  
  1830.                 const MNM::vector3_t origin = MNM::vector3_t(MNM::real_t(paramsGrid.origin.x), MNM::real_t(paramsGrid.origin.y), MNM::real_t(paramsGrid.origin.z));
  1831.                 const Vec3& voxelSize = mesh.navMesh.GetGridParams().voxelSize;
  1832.                 const MNM::vector3_t seedLocation(MNM::real_t(debugLocation.x), MNM::real_t(debugLocation.y), MNM::real_t(debugLocation.z));
  1833.  
  1834.                 const uint16 agentHeightUnits = GetAgentHeightInVoxelUnits(agentTypeIdForAccessibilityCalculation);
  1835.  
  1836.                 const MNM::real_t verticalRange = MNMUtils::CalculateMinVerticalRange(agentHeightUnits, voxelSize.z);
  1837.                 const MNM::real_t verticalDownwardRange(verticalRange);
  1838.  
  1839.                 AgentType agentTypeProperties;
  1840.                 const bool arePropertiesValid = GetAgentTypeProperties(agentTypeIdForAccessibilityCalculation, agentTypeProperties);
  1841.                 assert(arePropertiesValid);
  1842.                 const uint16 minZOffsetMultiplier(2);
  1843.                 const uint16 zOffsetMultiplier = min(minZOffsetMultiplier, agentTypeProperties.settings.heightVoxelCount);
  1844.                 const MNM::real_t verticalUpwardRange = arePropertiesValid ? MNM::real_t(zOffsetMultiplier * agentTypeProperties.settings.voxelSize.z) : MNM::real_t(.2f);
  1845.  
  1846.                 MNM::TriangleID seedTriangleID = mesh.navMesh.GetTriangleAt(seedLocation - origin, verticalDownwardRange, verticalUpwardRange);
  1847.  
  1848.                 if (seedTriangleID)
  1849.                 {
  1850.                         MNM::CNavMesh::AccessibilityRequest inputRequest(seedTriangleID, offMeshNavigation);
  1851.                         mesh.navMesh.ComputeAccessibility(inputRequest);
  1852.                 }
  1853.         }
  1854. }
  1855.  
  1856. void NavigationSystem::ClearAllAccessibility(uint8 resetValue)
  1857. {
  1858.         AgentTypes::const_iterator it = m_agentTypes.begin();
  1859.         AgentTypes::const_iterator end = m_agentTypes.end();
  1860.  
  1861.         for (; it != end; ++it)
  1862.         {
  1863.                 const AgentType& agentType = *it;
  1864.                 AgentType::Meshes::const_iterator itMesh = agentType.meshes.begin();
  1865.                 AgentType::Meshes::const_iterator endMesh = agentType.meshes.end();
  1866.                 for (; itMesh != endMesh; ++itMesh)
  1867.                 {
  1868.                         if (itMesh->id && m_meshes.validate(itMesh->id))
  1869.                         {
  1870.                                 NavigationMesh& mesh = m_meshes[itMesh->id];
  1871.                                 mesh.navMesh.ResetAccessibility(resetValue);
  1872.                         }
  1873.                 }
  1874.         }
  1875. }
  1876. #endif
  1877.  
  1878. void NavigationSystem::CalculateAccessibility()
  1879. {
  1880. #if MNM_USE_EXPORT_INFORMATION
  1881.  
  1882.         bool isThereAtLeastOneSeedPresent = false;
  1883.  
  1884.         ClearAllAccessibility(MNM::CNavMesh::eARNotAccessible);
  1885.  
  1886.         // Filtering accessibility with actors
  1887.         {
  1888.                 AutoAIObjectIter itActors(gAIEnv.pAIObjectManager->GetFirstAIObject(OBJFILTER_TYPE, AIOBJECT_ACTOR));
  1889.  
  1890.                 for (; itActors->GetObject(); itActors->Next())
  1891.                 {
  1892.                         ComputeAccessibility(itActors->GetObject());
  1893.                 }
  1894.         }
  1895.  
  1896.         // Filtering accessibility with Navigation Seeds
  1897.         {
  1898.                 AutoAIObjectIter itNavSeeds(gAIEnv.pAIObjectManager->GetFirstAIObject(OBJFILTER_TYPE, AIOBJECT_NAV_SEED));
  1899.  
  1900.                 for (; itNavSeeds->GetObject(); itNavSeeds->Next())
  1901.                 {
  1902.  
  1903.                         AgentTypes::const_iterator it = m_agentTypes.begin();
  1904.                         AgentTypes::const_iterator end = m_agentTypes.end();
  1905.  
  1906.                         for (; it != end; ++it)
  1907.                         {
  1908.                                 const AgentType& agentType = *it;
  1909.                                 ComputeAccessibility(itNavSeeds->GetObject(), GetAgentTypeID(it->name));
  1910.                         }
  1911.                 }
  1912.         }
  1913. #endif
  1914. }
  1915.  
  1916. bool NavigationSystem::IsInUse() const
  1917. {
  1918.         return m_meshes.size() != 0;
  1919. }
  1920.  
  1921. MNM::TileID NavigationSystem::GetTileIdWhereLocationIsAtForMesh(NavigationMeshID meshID, const Vec3& location)
  1922. {
  1923.         NavigationMesh& mesh = GetMesh(meshID);
  1924.  
  1925.         const MNM::real_t range = MNM::real_t(1.0f);
  1926.         MNM::TriangleID triangleID = mesh.navMesh.GetTriangleAt(location, range, range);
  1927.  
  1928.         return MNM::ComputeTileID(triangleID);
  1929. }
  1930.  
  1931. void NavigationSystem::GetTileBoundsForMesh(NavigationMeshID meshID, MNM::TileID tileID, AABB& bounds) const
  1932. {
  1933.         const NavigationMesh& mesh = GetMesh(meshID);
  1934.         const MNM::vector3_t coords = mesh.navMesh.GetTileContainerCoordinates(tileID);
  1935.  
  1936.         const MNM::CNavMesh::SGridParams& params = mesh.navMesh.GetGridParams();
  1937.  
  1938.         Vec3 minPos((float)params.tileSize.x * coords.x.as_float(), (float)params.tileSize.y * coords.y.as_float(), (float)params.tileSize.z * coords.z.as_float());
  1939.         minPos += params.origin;
  1940.  
  1941.         bounds = AABB(minPos, minPos + Vec3((float)params.tileSize.x, (float)params.tileSize.y, (float)params.tileSize.z));
  1942. }
  1943.  
  1944. NavigationMesh& NavigationSystem::GetMesh(const NavigationMeshID& meshID)
  1945. {
  1946.         return const_cast<NavigationMesh&>(static_cast<const NavigationSystem*>(this)->GetMesh(meshID));
  1947. }
  1948.  
  1949. const NavigationMesh& NavigationSystem::GetMesh(const NavigationMeshID& meshID) const
  1950. {
  1951.         if (meshID && m_meshes.validate(meshID))
  1952.                 return m_meshes[meshID];
  1953.  
  1954.         assert(0);
  1955.         static NavigationMesh dummy(NavigationAgentTypeID(0));
  1956.  
  1957.         return dummy;
  1958. }
  1959.  
  1960. NavigationMeshID NavigationSystem::GetEnclosingMeshID(NavigationAgentTypeID agentTypeID, const Vec3& location) const
  1961. {
  1962.         if (agentTypeID && agentTypeID <= m_agentTypes.size())
  1963.         {
  1964.                 const AgentType& agentType = m_agentTypes[agentTypeID - 1];
  1965.  
  1966.                 AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  1967.                 AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  1968.  
  1969.                 for (; mit != mend; ++mit)
  1970.                 {
  1971.                         const NavigationMeshID meshID = mit->id;
  1972.                         const NavigationMesh& mesh = m_meshes[meshID];
  1973.                         const NavigationVolumeID boundaryID = mesh.boundary;
  1974.  
  1975.                         if (boundaryID && m_volumes[boundaryID].Contains(location))
  1976.                                 return meshID;
  1977.                 }
  1978.         }
  1979.  
  1980.         return NavigationMeshID();
  1981. }
  1982.  
  1983. bool NavigationSystem::IsLocationInMesh(NavigationMeshID meshID, const Vec3& location) const
  1984. {
  1985.         if (meshID && m_meshes.validate(meshID))
  1986.         {
  1987.                 const NavigationMesh& mesh = m_meshes[meshID];
  1988.                 const NavigationVolumeID boundaryID = mesh.boundary;
  1989.  
  1990.                 return (boundaryID && m_volumes[boundaryID].Contains(location));
  1991.         }
  1992.  
  1993.         return false;
  1994. }
  1995.  
  1996. MNM::TriangleID NavigationSystem::GetClosestMeshLocation(NavigationMeshID meshID, const Vec3& location, float vrange,
  1997.                                                          float hrange, Vec3* meshLocation, float* distance) const
  1998. {
  1999.         if (meshID && m_meshes.validate(meshID))
  2000.         {
  2001.                 MNM::vector3_t loc(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z));
  2002.                 const NavigationMesh& mesh = m_meshes[meshID];
  2003.                 MNM::real_t verticalRange(vrange);
  2004.                 if (const MNM::TriangleID enclosingTriID = mesh.navMesh.GetTriangleAt(loc, verticalRange, verticalRange))
  2005.                 {
  2006.                         if (meshLocation)
  2007.                                 *meshLocation = location;
  2008.  
  2009.                         if (distance)
  2010.                                 *distance = 0.0f;
  2011.  
  2012.                         return enclosingTriID;
  2013.                 }
  2014.                 else
  2015.                 {
  2016.                         MNM::real_t distanceFixed;
  2017.                         MNM::vector3_t closest;
  2018.  
  2019.                         if (const MNM::TriangleID closestTriID = mesh.navMesh.GetClosestTriangle(loc, MNM::real_t(vrange), MNM::real_t(hrange), &distanceFixed, &closest))
  2020.                         {
  2021.                                 if (meshLocation)
  2022.                                         *meshLocation = closest.GetVec3();
  2023.  
  2024.                                 if (distance)
  2025.                                         *distance = distanceFixed.as_float();
  2026.  
  2027.                                 return closestTriID;
  2028.                         }
  2029.                 }
  2030.         }
  2031.  
  2032.         return MNM::TriangleID(0);
  2033. }
  2034.  
  2035. bool NavigationSystem::GetGroundLocationInMesh(NavigationMeshID meshID, const Vec3& location,
  2036.                                                float vDownwardRange, float hRange, Vec3* meshLocation) const
  2037. {
  2038.         if (meshID && m_meshes.validate(meshID))
  2039.         {
  2040.                 MNM::vector3_t loc(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z));
  2041.                 const NavigationMesh& mesh = m_meshes[meshID];
  2042.                 MNM::real_t verticalRange(vDownwardRange);
  2043.                 if (const MNM::TriangleID enclosingTriID = mesh.navMesh.GetTriangleAt(loc, verticalRange, MNM::real_t(0.05f)))
  2044.                 {
  2045.                         MNM::vector3_t v0, v1, v2;
  2046.                         mesh.navMesh.GetVertices(enclosingTriID, v0, v1, v2);
  2047.                         MNM::vector3_t closest = ClosestPtPointTriangle(loc, v0, v1, v2);
  2048.                         if (meshLocation)
  2049.                                 *meshLocation = closest.GetVec3();
  2050.  
  2051.                         return true;
  2052.                 }
  2053.                 else
  2054.                 {
  2055.                         MNM::vector3_t closest;
  2056.  
  2057.                         if (const MNM::TriangleID closestTriID = mesh.navMesh.GetClosestTriangle(loc, verticalRange, MNM::real_t(hRange), nullptr, &closest))
  2058.                         {
  2059.                                 if (meshLocation)
  2060.                                         *meshLocation = closest.GetVec3();
  2061.  
  2062.                                 return true;
  2063.                         }
  2064.                 }
  2065.         }
  2066.  
  2067.         return false;
  2068. }
  2069.  
  2070. bool NavigationSystem::AgentTypeSupportSmartObjectUserClass(NavigationAgentTypeID agentTypeID, const char* smartObjectUserClass) const
  2071. {
  2072.         if (agentTypeID && agentTypeID <= m_agentTypes.size())
  2073.         {
  2074.                 const AgentType& agentType = m_agentTypes[agentTypeID - 1];
  2075.                 AgentType::SmartObjectUserClasses::const_iterator cit = agentType.smartObjectUserClasses.begin();
  2076.                 AgentType::SmartObjectUserClasses::const_iterator end = agentType.smartObjectUserClasses.end();
  2077.  
  2078.                 for (; cit != end; ++cit)
  2079.                 {
  2080.                         if (strcmp((*cit).c_str(), smartObjectUserClass))
  2081.                         {
  2082.                                 continue;
  2083.                         }
  2084.  
  2085.                         return true;
  2086.                 }
  2087.         }
  2088.  
  2089.         return false;
  2090. }
  2091.  
  2092. uint16 NavigationSystem::GetAgentRadiusInVoxelUnits(NavigationAgentTypeID agentTypeID) const
  2093. {
  2094.         if (agentTypeID && agentTypeID <= m_agentTypes.size())
  2095.         {
  2096.                 return m_agentTypes[agentTypeID - 1].settings.radiusVoxelCount;
  2097.         }
  2098.  
  2099.         return 0;
  2100. }
  2101.  
  2102. uint16 NavigationSystem::GetAgentHeightInVoxelUnits(NavigationAgentTypeID agentTypeID) const
  2103. {
  2104.         if (agentTypeID && agentTypeID <= m_agentTypes.size())
  2105.         {
  2106.                 return m_agentTypes[agentTypeID - 1].settings.heightVoxelCount;
  2107.         }
  2108.  
  2109.         return 0;
  2110. }
  2111.  
  2112. void NavigationSystem::Clear()
  2113. {
  2114.         StopAllTasks();
  2115.         SetupTasks();
  2116.  
  2117.         AgentTypes::iterator it = m_agentTypes.begin();
  2118.         AgentTypes::iterator end = m_agentTypes.end();
  2119.  
  2120.         for (; it != end; ++it)
  2121.         {
  2122.                 AgentType& agentType = *it;
  2123.                 agentType.meshes.clear();
  2124.                 agentType.exclusions.clear();
  2125.         }
  2126.  
  2127.         for (uint16 i = 0; i < m_meshes.capacity(); ++i)
  2128.         {
  2129.                 if (!m_meshes.index_free(i))
  2130.                         DestroyMesh(NavigationMeshID(m_meshes.get_index_id(i)));
  2131.         }
  2132.  
  2133.         for (uint16 i = 0; i < m_volumes.capacity(); ++i)
  2134.         {
  2135.                 if (!m_volumes.index_free(i))
  2136.                         DestroyVolume(NavigationVolumeID(m_volumes.get_index_id(i)));
  2137.         }
  2138.  
  2139.         m_volumesManager.Clear();
  2140.  
  2141. #ifdef SW_NAVMESH_USE_GUID
  2142.         m_swMeshes.clear();
  2143.         m_swVolumes.clear();
  2144.  
  2145.         m_nextFreeMeshId = 0;
  2146.         m_nextFreeVolumeId = 0;
  2147. #endif
  2148.  
  2149.         m_worldAABB = AABB::RESET;
  2150.  
  2151.         m_tileQueue.clear();
  2152.         m_volumeDefCopy.clear();
  2153.         m_volumeDefCopy.resize(MaxVolumeDefCopyCount, VolumeDefCopy());
  2154.  
  2155.         m_offMeshNavigationManager.Clear();
  2156.         m_islandConnectionsManager.Reset();
  2157.  
  2158.         ResetAllNavigationSystemUsers();
  2159. }
  2160.  
  2161. void NavigationSystem::ClearAndNotify()
  2162. {
  2163.         Clear();
  2164.         UpdateAllListener(NavigationCleared);
  2165.  
  2166.         //////////////////////////////////////////////////////////////////////////
  2167.         //After the 'clear' we need to re-enable and register smart objects again
  2168.  
  2169.         m_offMeshNavigationManager.Enable();
  2170.         gAIEnv.pSmartObjectManager->SoftReset();
  2171. }
  2172.  
  2173. bool NavigationSystem::ReloadConfig()
  2174. {
  2175.         // TODO pavloi 2016.03.09: this function should be refactored.
  2176.         // Proper way of doing things should be something like this:
  2177.         // 1) load config data into an array of CreateAgentTypeParams, do nothing, if there are errors
  2178.         // 2) pass array of CreateAgentTypeParams into different publically available function, which will:
  2179.         //    a) notify interested listeners, that we're about to wipe out and reload navigation;
  2180.         //    b) clear current data;
  2181.         //    c) replace agent settings;
  2182.         //    d) kick-off navmesh regeneration (if required/enabled);
  2183.         //    e) notify listeners, that new settings are available.
  2184.         // This way:
  2185.         // 1) loading and application of config will be decoupled from each other, so we will be able to put data in descriptor database (if we want);
  2186.         // 2) it will be possible to reliably reload config and provide better editor tools, which react to config changes in runtime;
  2187.         // 3) continuous nav mesh update can be taken away from editor's CAIManager to navigation system.
  2188.  
  2189.         Clear();
  2190.  
  2191.         m_agentTypes.clear();
  2192.  
  2193.         XmlNodeRef rootNode = GetISystem()->LoadXmlFromFile(m_configName.c_str());
  2194.         if (!rootNode)
  2195.         {
  2196.                 AIWarning("Failed to open XML file '%s'...", m_configName.c_str());
  2197.  
  2198.                 return false;
  2199.         }
  2200.  
  2201.         const char* tagName = rootNode->getTag();
  2202.  
  2203.         if (!stricmp(tagName, "Navigation"))
  2204.         {
  2205.                 rootNode->getAttr("version", m_configurationVersion);
  2206.  
  2207.                 size_t childCount = rootNode->getChildCount();
  2208.  
  2209.                 for (size_t i = 0; i < childCount; ++i)
  2210.                 {
  2211.                         XmlNodeRef childNode = rootNode->getChild(i);
  2212.  
  2213.                         if (!stricmp(childNode->getTag(), "AgentTypes"))
  2214.                         {
  2215.                                 size_t agentTypeCount = childNode->getChildCount();
  2216.  
  2217.                                 for (size_t at = 0; at < agentTypeCount; ++at)
  2218.                                 {
  2219.                                         XmlNodeRef agentTypeNode = childNode->getChild(at);
  2220.  
  2221.                                         if (!agentTypeNode->haveAttr("name"))
  2222.                                         {
  2223.                                                 AIWarning("Missing 'name' attribute for 'AgentType' tag at line %d while parsing NavigationXML '%s'...",
  2224.                                                           agentTypeNode->getLine(), m_configName.c_str());
  2225.  
  2226.                                                 return false;
  2227.                                         }
  2228.  
  2229.                                         const char* name = 0;
  2230.                                         INavigationSystem::CreateAgentTypeParams params;
  2231.  
  2232.                                         for (size_t attr = 0; attr < (size_t)agentTypeNode->getNumAttributes(); ++attr)
  2233.                                         {
  2234.                                                 const char* attrName = 0;
  2235.                                                 const char* attrValue = 0;
  2236.  
  2237.                                                 if (!agentTypeNode->getAttributeByIndex(attr, &attrName, &attrValue))
  2238.                                                         continue;
  2239.  
  2240.                                                 bool valid = false;
  2241.                                                 if (!stricmp(attrName, "name"))
  2242.                                                 {
  2243.                                                         if (attrValue && *attrValue)
  2244.                                                         {
  2245.                                                                 valid = true;
  2246.                                                                 name = attrValue;
  2247.                                                         }
  2248.                                                 }
  2249.                                                 else if (!stricmp(attrName, "radius"))
  2250.                                                 {
  2251.                                                         int sradius = 0;
  2252.                                                         if (sscanf(attrValue, "%d", &sradius) == 1 && sradius > 0)
  2253.                                                         {
  2254.                                                                 valid = true;
  2255.                                                                 params.radiusVoxelCount = sradius;
  2256.                                                         }
  2257.                                                 }
  2258.                                                 else if (!stricmp(attrName, "height"))
  2259.                                                 {
  2260.                                                         int sheight = 0;
  2261.                                                         if (sscanf(attrValue, "%d", &sheight) == 1 && sheight > 0)
  2262.                                                         {
  2263.                                                                 valid = true;
  2264.                                                                 params.heightVoxelCount = sheight;
  2265.                                                         }
  2266.                                                 }
  2267.                                                 else if (!stricmp(attrName, "climbableHeight"))
  2268.                                                 {
  2269.                                                         int sclimbableheight = 0;
  2270.                                                         if (sscanf(attrValue, "%d", &sclimbableheight) == 1 && sclimbableheight >= 0)
  2271.                                                         {
  2272.                                                                 valid = true;
  2273.                                                                 params.climbableVoxelCount = sclimbableheight;
  2274.                                                         }
  2275.                                                 }
  2276.                                                 else if (!stricmp(attrName, "climbableInclineGradient"))
  2277.                                                 {
  2278.                                                         float sclimbableinclinegradient = 0.f;
  2279.                                                         if (sscanf(attrValue, "%f", &sclimbableinclinegradient) == 1)
  2280.                                                         {
  2281.                                                                 valid = true;
  2282.                                                                 params.climbableInclineGradient = sclimbableinclinegradient;
  2283.                                                         }
  2284.                                                 }
  2285.                                                 else if (!stricmp(attrName, "climbableStepRatio"))
  2286.                                                 {
  2287.                                                         float sclimbablestepratio = 0.f;
  2288.                                                         if (sscanf(attrValue, "%f", &sclimbablestepratio) == 1)
  2289.                                                         {
  2290.                                                                 valid = true;
  2291.                                                                 params.climbableStepRatio = sclimbablestepratio;
  2292.                                                         }
  2293.                                                 }
  2294.                                                 else if (!stricmp(attrName, "maxWaterDepth"))
  2295.                                                 {
  2296.                                                         int smaxwaterdepth = 0;
  2297.                                                         if (sscanf(attrValue, "%d", &smaxwaterdepth) == 1 && smaxwaterdepth >= 0)
  2298.                                                         {
  2299.                                                                 valid = true;
  2300.                                                                 params.maxWaterDepthVoxelCount = smaxwaterdepth;
  2301.                                                         }
  2302.                                                 }
  2303.                                                 else if (!stricmp(attrName, "voxelSize"))
  2304.                                                 {
  2305.                                                         float x, y, z;
  2306.                                                         int c = sscanf(attrValue, "%g,%g,%g", &x, &y, &z);
  2307.  
  2308.                                                         valid = (c == 1) || (c == 3);
  2309.                                                         if (c == 1)
  2310.                                                                 params.voxelSize = Vec3(x);
  2311.                                                         else if (c == 3)
  2312.                                                                 params.voxelSize = Vec3(x, y, z);
  2313.                                                 }
  2314.                                                 else
  2315.                                                 {
  2316.                                                         AIWarning(
  2317.                                                           "Unknown attribute '%s' for '%s' tag found at line %d while parsing Navigation XML '%s'...",
  2318.                                                           attrName, agentTypeNode->getTag(), agentTypeNode->getLine(), m_configName.c_str());
  2319.  
  2320.                                                         return false;
  2321.                                                 }
  2322.  
  2323.                                                 if (!valid)
  2324.                                                 {
  2325.                                                         AIWarning("Invalid '%s' attribute value for '%s' tag at line %d while parsing NavigationXML '%s'...",
  2326.                                                                   attrName, agentTypeNode->getTag(), agentTypeNode->getLine(), m_configName.c_str());
  2327.  
  2328.                                                         return false;
  2329.                                                 }
  2330.                                         }
  2331.  
  2332.                                         for (size_t childIdx = 0; childIdx < m_agentTypes.size(); ++childIdx)
  2333.                                         {
  2334.                                                 const AgentType& agentType = m_agentTypes[childIdx];
  2335.  
  2336.                                                 assert(name);
  2337.  
  2338.                                                 if (!stricmp(agentType.name.c_str(), name))
  2339.                                                 {
  2340.                                                         AIWarning("AgentType '%s' redefinition at line %d while parsing NavigationXML '%s'...",
  2341.                                                                   name, agentTypeNode->getLine(), m_configName.c_str());
  2342.  
  2343.                                                         return false;
  2344.                                                 }
  2345.                                         }
  2346.  
  2347.                                         NavigationAgentTypeID agentTypeID = CreateAgentType(name, params);
  2348.                                         if (!agentTypeID)
  2349.                                                 return false;
  2350.  
  2351.                                         //////////////////////////////////////////////////////////////////////////
  2352.                                         /// Add supported SO classes for this AgentType/Mesh
  2353.  
  2354.                                         for (size_t childIdx = 0; childIdx < (size_t)agentTypeNode->getChildCount(); ++childIdx)
  2355.                                         {
  2356.                                                 XmlNodeRef agentTypeChildNode = agentTypeNode->getChild(childIdx);
  2357.  
  2358.                                                 if (!stricmp(agentTypeChildNode->getTag(), "SmartObjectUserClasses"))
  2359.                                                 {
  2360.                                                         AgentType& agentType = m_agentTypes[agentTypeID - 1];
  2361.  
  2362.                                                         size_t soClassesCount = agentTypeChildNode->getChildCount();
  2363.                                                         agentType.smartObjectUserClasses.reserve(soClassesCount);
  2364.  
  2365.                                                         for (size_t socIdx = 0; socIdx < soClassesCount; ++socIdx)
  2366.                                                         {
  2367.                                                                 XmlNodeRef smartObjectClassNode = agentTypeChildNode->getChild(socIdx);
  2368.  
  2369.                                                                 if (!stricmp(smartObjectClassNode->getTag(), "class") && smartObjectClassNode->haveAttr("name"))
  2370.                                                                 {
  2371.                                                                         stl::push_back_unique(agentType.smartObjectUserClasses, smartObjectClassNode->getAttr("name"));
  2372.                                                                 }
  2373.                                                         }
  2374.                                                 }
  2375.                                         }
  2376.                                 }
  2377.                         }
  2378.                         else
  2379.                         {
  2380.                                 AIWarning(
  2381.                                   "Unexpected tag '%s' found at line %d while parsing Navigation XML '%s'...",
  2382.                                   childNode->getTag(), childNode->getLine(), m_configName.c_str());
  2383.  
  2384.                                 return false;
  2385.                         }
  2386.                 }
  2387.  
  2388.                 return true;
  2389.         }
  2390.         else
  2391.         {
  2392.                 AIWarning(
  2393.                   "Unexpected tag '%s' found at line %d while parsing Navigation XML '%s'...",
  2394.                   rootNode->getTag(), rootNode->getLine(), m_configName.c_str());
  2395.         }
  2396.  
  2397.         return false;
  2398. }
  2399.  
  2400. void NavigationSystem::ComputeWorldAABB()
  2401. {
  2402.         AgentTypes::const_iterator it = m_agentTypes.begin();
  2403.         AgentTypes::const_iterator end = m_agentTypes.end();
  2404.  
  2405.         m_worldAABB = AABB(AABB::RESET);
  2406.  
  2407.         for (; it != end; ++it)
  2408.         {
  2409.                 const AgentType& agentType = *it;
  2410.  
  2411.                 AgentType::Meshes::const_iterator mit = agentType.meshes.begin();
  2412.                 AgentType::Meshes::const_iterator mend = agentType.meshes.end();
  2413.  
  2414.                 for (; mit != mend; ++mit)
  2415.                 {
  2416.                         const NavigationMeshID meshID = mit->id;
  2417.                         const NavigationMesh& mesh = m_meshes[meshID];
  2418.  
  2419.                         if (mesh.boundary)
  2420.                                 m_worldAABB.Add(m_volumes[mesh.boundary].aabb);
  2421.                 }
  2422.         }
  2423. }
  2424.  
  2425. void NavigationSystem::SetupTasks()
  2426. {
  2427.         // the number of parallel tasks while allowing maximization of tile throughput
  2428.         // might also slow down the main thread due to extra time processing them but also because
  2429.         // the connection of each tile to network is done on the main thread
  2430.         // NOTE ChrisR: capped to half the amount. The execution time of a tile job desperately needs to optimized,
  2431.         // it cannot not take more than the frame time because it'll stall the system. No clever priority scheme
  2432.         // will ever,ever,ever make that efficient.
  2433.         // PeteB: Optimized the tile job time enough to make it viable to do more than one per frame. Added CVar
  2434.         //        multiplier to allow people to control it based on the speed of their machine.
  2435.         m_maxRunningTaskCount = (gEnv->pJobManager->GetNumWorkerThreads() * 3 / 4) * gAIEnv.CVars.NavGenThreadJobs;
  2436.         m_results.resize(m_maxRunningTaskCount);
  2437.  
  2438.         for (uint16 i = 0; i < m_results.size(); ++i)
  2439.                 m_results[i].next = i + 1;
  2440.  
  2441.         m_runningTasks.reserve(m_maxRunningTaskCount);
  2442.  
  2443.         m_free = 0;
  2444. }
  2445.  
  2446. bool NavigationSystem::RegisterArea(const char* shapeName, NavigationVolumeID& outVolumeId)
  2447. {
  2448.         return m_volumesManager.RegisterArea(shapeName, outVolumeId);
  2449. }
  2450.  
  2451. void NavigationSystem::UnRegisterArea(const char* shapeName)
  2452. {
  2453.         m_volumesManager.UnRegisterArea(shapeName);
  2454. }
  2455.  
  2456. NavigationVolumeID NavigationSystem::GetAreaId(const char* shapeName) const
  2457. {
  2458.         return m_volumesManager.GetAreaID(shapeName);
  2459. }
  2460.  
  2461. void NavigationSystem::SetAreaId(const char* shapeName, NavigationVolumeID id)
  2462. {
  2463.         m_volumesManager.SetAreaID(shapeName, id);
  2464. }
  2465.  
  2466. void NavigationSystem::UpdateAreaNameForId(const NavigationVolumeID id, const char* newShapeName)
  2467. {
  2468.         m_volumesManager.UpdateNameForAreaID(id, newShapeName);
  2469. }
  2470.  
  2471. void NavigationSystem::RemoveLoadedMeshesWithoutRegisteredAreas()
  2472. {
  2473.         std::vector<NavigationVolumeID> volumesToRemove;
  2474.         m_volumesManager.GetLoadedUnregisteredVolumes(volumesToRemove);
  2475.  
  2476.         if (!volumesToRemove.empty())
  2477.         {
  2478.                 std::vector<NavigationMeshID> meshesToRemove;
  2479.  
  2480.                 for (const NavigationVolumeID& volumeId : volumesToRemove)
  2481.                 {
  2482.                         AILogComment("Removing Navigation Volume id = %u because it was created by loaded unregistered navigation area.",
  2483.                                      (uint32)volumeId);
  2484.  
  2485.                         for (const AgentType& agentType : m_agentTypes)
  2486.                         {
  2487.                                 for (const AgentType::MeshInfo& meshInfo : agentType.meshes)
  2488.                                 {
  2489.                                         const NavigationMeshID meshID = meshInfo.id;
  2490.                                         const NavigationMesh& mesh = m_meshes[meshID];
  2491.  
  2492.                                         if (mesh.boundary == volumeId)
  2493.                                         {
  2494.                                                 meshesToRemove.push_back(meshID);
  2495.                                                 AILogComment("Removing NavMesh '%s' (meshId = %u, agent = %s) because it uses loaded unregistered navigation area id = %u.",
  2496.                                                              mesh.name.c_str(), (uint32)meshID, agentType.name.c_str(), (uint32)volumeId);
  2497.                                         }
  2498.                                 }
  2499.                         }
  2500.                 }
  2501.  
  2502.                 for (const NavigationMeshID& meshId : meshesToRemove)
  2503.                 {
  2504.                         DestroyMesh(meshId);
  2505.                 }
  2506.  
  2507.                 for (const NavigationVolumeID& volumeId : volumesToRemove)
  2508.                 {
  2509.                         DestroyVolume(volumeId);
  2510.                 }
  2511.         }
  2512.  
  2513.         m_volumesManager.ClearLoadedAreas();
  2514. }
  2515.  
  2516. void NavigationSystem::StartWorldMonitoring()
  2517. {
  2518.         m_worldMonitor.Start();
  2519. }
  2520.  
  2521. void NavigationSystem::StopWorldMonitoring()
  2522. {
  2523.         m_worldMonitor.Stop();
  2524. }
  2525.  
  2526. bool NavigationSystem::GetClosestPointInNavigationMesh(const NavigationAgentTypeID agentID, const Vec3& location, float vrange, float hrange, Vec3* meshLocation, float minIslandArea) const
  2527. {
  2528.         const NavigationMeshID meshID = GetEnclosingMeshID(agentID, location);
  2529.         if (meshID && m_meshes.validate(meshID))
  2530.         {
  2531.                 MNM::vector3_t loc(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z));
  2532.                 const NavigationMesh& mesh = m_meshes[meshID];
  2533.                 MNM::real_t verticalRange(vrange);
  2534.  
  2535.                 //first check vertical range, because if we are over navmesh, we want that one
  2536.                 if (const MNM::TriangleID enclosingTriID = mesh.navMesh.GetTriangleAt(loc, verticalRange, verticalRange, minIslandArea))
  2537.                 {
  2538.                         MNM::vector3_t v0, v1, v2;
  2539.                         mesh.navMesh.GetVertices(enclosingTriID, v0, v1, v2);
  2540.                         MNM::vector3_t closest = ClosestPtPointTriangle(loc, v0, v1, v2);
  2541.  
  2542.                         if (meshLocation)
  2543.                         {
  2544.                                 *meshLocation = closest.GetVec3();
  2545.                         }
  2546.  
  2547.                         return true;
  2548.                 }
  2549.                 else
  2550.                 {
  2551.                         MNM::vector3_t closest;
  2552.  
  2553.                         if (const MNM::TriangleID closestTriID = mesh.navMesh.GetClosestTriangle(loc, MNM::real_t(vrange), MNM::real_t(hrange), nullptr, &closest, minIslandArea))
  2554.                         {
  2555.                                 if (meshLocation)
  2556.                                 {
  2557.                                         *meshLocation = closest.GetVec3();
  2558.                                 }
  2559.  
  2560.                                 return true;
  2561.                         }
  2562.                 }
  2563.         }
  2564.  
  2565.         return false;
  2566. }
  2567.  
  2568. bool NavigationSystem::IsLocationValidInNavigationMesh(const NavigationAgentTypeID agentID, const Vec3& location) const
  2569. {
  2570.         const NavigationMeshID meshID = GetEnclosingMeshID(agentID, location);
  2571.         bool isValid = false;
  2572.         const float horizontalRange = 1.0f;
  2573.         const float verticalRange = 1.0f;
  2574.         if (meshID)
  2575.         {
  2576.                 Vec3 locationInMesh(ZERO);
  2577.                 float accuracy = .0f;
  2578.                 const MNM::TriangleID triangleID = GetClosestMeshLocation(meshID, location, verticalRange, horizontalRange, &locationInMesh, &accuracy);
  2579.                 isValid = (triangleID && accuracy == .0f);
  2580.         }
  2581.         return isValid;
  2582. }
  2583.  
  2584. bool NavigationSystem::IsPointReachableFromPosition(const NavigationAgentTypeID agentID, const IEntity* pEntityToTestOffGridLinks, const Vec3& startLocation, const Vec3& endLocation) const
  2585. {
  2586.         const float horizontalRange = 1.0f;
  2587.         const float verticalRange = 1.0f;
  2588.  
  2589.         MNM::GlobalIslandID startingIslandID;
  2590.         const NavigationMeshID startingMeshID = GetEnclosingMeshID(agentID, startLocation);
  2591.         if (startingMeshID)
  2592.         {
  2593.                 const MNM::TriangleID triangleID = GetClosestMeshLocation(startingMeshID, startLocation, verticalRange, horizontalRange, NULL, NULL);
  2594.                 const NavigationMesh& mesh = m_meshes[startingMeshID];
  2595.                 const MNM::CNavMesh& navMesh = mesh.navMesh;
  2596.                 MNM::Tile::STriangle triangle;
  2597.                 if (triangleID && navMesh.GetTriangle(triangleID, triangle) && (triangle.islandID != MNM::Constants::eStaticIsland_InvalidIslandID))
  2598.                 {
  2599.                         startingIslandID = MNM::GlobalIslandID(startingMeshID, triangle.islandID);
  2600.                 }
  2601.         }
  2602.  
  2603.         MNM::GlobalIslandID endingIslandID;
  2604.         const NavigationMeshID endingMeshID = GetEnclosingMeshID(agentID, endLocation);
  2605.         if (endingMeshID)
  2606.         {
  2607.                 const MNM::TriangleID triangleID = GetClosestMeshLocation(endingMeshID, endLocation, verticalRange, horizontalRange, NULL, NULL);
  2608.                 const NavigationMesh& mesh = m_meshes[endingMeshID];
  2609.                 const MNM::CNavMesh& navMesh = mesh.navMesh;