BVB Source Codes

CRYENGINE Show NavMesh.cpp Source code

Return Download CRYENGINE: download NavMesh.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 "NavMesh.h"
  5. #include "OffGridLinks.h"
  6. #include "Tile.h"
  7. #include "IslandConnections.h"
  8. #include "../NavigationSystem/OffMeshNavigationManager.h"
  9.  
  10. #if defined(min)
  11.         #undef min
  12. #endif
  13. #if defined(max)
  14.         #undef max
  15. #endif
  16.  
  17. // - if this is #defined, then some public methods of CNavMesh::TileContainerArray will receive additional asserts() to debug access to freed TileContainer instances; this might
  18. //   generate false positives while we're in the process of tracing down bugs though
  19. // - if not #defined, then the code will effectively behave the same as before 2015-08-13 (i. e. it will *not* do these extra checks, thus not generating false positives)
  20. //#define TILE_CONTAINER_ARRAY_STRICT_ACCESS_CHECKS
  21.  
  22. //#pragma optimize("", off)
  23. //#pragma inline_depth(0)
  24.  
  25. namespace MNM
  26. {
  27. bool CNavMesh::WayQueryRequest::CanUseOffMeshLink(const OffMeshLinkID linkID, float* costMultiplier) const
  28. {
  29.         if (m_pRequester)
  30.         {
  31.                 if (IEntity* pEntity = m_pRequester->GetPathAgentEntity())
  32.                 {
  33.                         if (const OffMeshLink* pOffMeshLink = m_offMeshNavigationManager.GetOffMeshLink(linkID))
  34.                         {
  35.                                 return pOffMeshLink->CanUse(pEntity, costMultiplier);
  36.                         }
  37.                 }
  38.         }
  39.  
  40.         return true;    // Always allow by default
  41. }
  42.  
  43. bool CNavMesh::WayQueryRequest::IsPointValidForAgent(const Vec3& pos, uint32 flags) const
  44. {
  45.         if (m_pRequester)
  46.         {
  47.                 return m_pRequester->IsPointValidForAgent(pos, flags);
  48.         }
  49.  
  50.         return true;    // Always allow by default
  51. }
  52.  
  53. CNavMesh::TileContainerArray::TileContainerArray()
  54.         : m_tiles(NULL)
  55.         , m_tileCount(0)
  56.         , m_tileCapacity(0)
  57. {
  58. }
  59.  
  60. CNavMesh::TileContainerArray::TileContainerArray(const TileContainerArray& rhs)
  61.         : m_tiles(NULL)
  62.         , m_tileCount(0)
  63.         , m_tileCapacity(0)
  64. {
  65.         // if this assert() triggers, then we must completely re-think the use-cases in which copy-construction can occur
  66.         assert(rhs.m_tiles == NULL);
  67. }
  68.  
  69. CNavMesh::TileContainerArray::~TileContainerArray()
  70. {
  71.         for (size_t i = 0; i < m_tileCapacity; ++i)
  72.                 m_tiles[i].tile.Destroy();
  73.  
  74.         delete[] m_tiles;
  75. }
  76.  
  77. void CNavMesh::TileContainerArray::Grow(size_t amount)
  78. {
  79.         const size_t oldCapacity = m_tileCapacity;
  80.  
  81.         m_tileCapacity += amount;
  82.         TileContainer* tiles = new TileContainer[m_tileCapacity];
  83.  
  84.         // careful #1: memcpy'ing is ok as Tile contains only primitive data types and pointers
  85.         // #MNM_TODO pavloi 2016.07.26: can't use std::is_trivially_copyable - unexpectedly it's not implemented for clang/gcc in our build configurations
  86.         //COMPILE_TIME_ASSERT(std::is_trivially_copyable<TileContainer>::value);
  87.         if (oldCapacity)
  88.                 memcpy(tiles, m_tiles, oldCapacity * sizeof(TileContainer));
  89.  
  90.         std::swap(m_tiles, tiles);
  91.  
  92.         // careful #2: don't worry, Tile has no dtor that would free memory pointed to by some of its members and suddenly leave parts of m_tiles[] dangling
  93.         //             (TileContainerArray::~TileContainerArray() will call Tile::Destroy() to free its memory)
  94.         delete[] tiles;
  95.  
  96.         // careful #3: we won't fill up m_frees as all elements starting at [m_frees.size() + m_tileCount] are implicitly marked as free.
  97. }
  98.  
  99. void CNavMesh::TileContainerArray::Init(size_t initialTileContainerCount)
  100. {
  101.         assert(m_tileCapacity == 0);
  102.         assert(m_tiles == NULL);
  103.         Grow(initialTileContainerCount);
  104. }
  105.  
  106. const TileID CNavMesh::TileContainerArray::AllocateTileContainer()
  107. {
  108.         assert(m_tileCount <= m_tileCapacity);
  109.  
  110.         ++m_tileCount;
  111.  
  112.         size_t tileID;
  113.  
  114.         if (m_freeIndexes.empty())
  115.         {
  116.                 tileID = m_tileCount;
  117.  
  118.                 if (m_tileCount > m_tileCapacity)
  119.                         Grow(std::max<size_t>(4, m_tileCapacity >> 1));
  120.         }
  121.         else
  122.         {
  123.                 tileID = m_freeIndexes.back() + 1;
  124.                 m_freeIndexes.pop_back();
  125.         }
  126.  
  127.         return static_cast<TileID>(tileID);
  128. }
  129.  
  130. void CNavMesh::TileContainerArray::FreeTileContainer(const TileID tileID)
  131. {
  132.         const size_t index = tileID - 1;
  133.  
  134.         assert(tileID > 0);
  135.         assert(tileID <= m_tileCapacity);
  136. #ifdef TILE_CONTAINER_ARRAY_STRICT_ACCESS_CHECKS
  137.         assert(!stl::find(m_freeIndexes, index));
  138.         assert(index < m_freeIndexes.size() + m_tileCount);
  139. #endif
  140.  
  141.         --m_tileCount;
  142.         m_freeIndexes.push_back(index);
  143. }
  144.  
  145. const size_t CNavMesh::TileContainerArray::GetTileCount() const
  146. {
  147.         return m_tileCount;
  148. }
  149.  
  150. CNavMesh::TileContainer& CNavMesh::TileContainerArray::operator[](size_t index)
  151. {
  152.         assert(index < m_tileCapacity);
  153. #ifdef TILE_CONTAINER_ARRAY_STRICT_ACCESS_CHECKS
  154.         assert(!stl::find(m_freeIndexes, index));
  155.         assert(index < m_freeIndexes.size() + m_tileCount);
  156. #endif
  157.         return m_tiles[index];
  158. }
  159.  
  160. const CNavMesh::TileContainer& CNavMesh::TileContainerArray::operator[](size_t index) const
  161. {
  162.         assert(index < m_tileCapacity);
  163. #ifdef TILE_CONTAINER_ARRAY_STRICT_ACCESS_CHECKS
  164.         assert(!stl::find(m_freeIndexes, index));
  165.         assert(index < m_freeIndexes.size() + m_tileCount);
  166. #endif
  167.         return m_tiles[index];
  168. }
  169.  
  170. void CNavMesh::TileContainerArray::Swap(TileContainerArray& other)
  171. {
  172.         std::swap(m_tiles, other.m_tiles);
  173.         std::swap(m_tileCount, other.m_tileCount);
  174.         std::swap(m_tileCapacity, other.m_tileCapacity);
  175.         m_freeIndexes.swap(other.m_freeIndexes);
  176. }
  177.  
  178. void CNavMesh::TileContainerArray::UpdateProfiler(ProfilerType& profiler) const
  179. {
  180.         profiler.AddMemory(GridMemory, m_tileCapacity * sizeof(TileContainer));
  181.         profiler.AddMemory(GridMemory, m_freeIndexes.capacity() * sizeof(FreeIndexes::value_type));
  182. }
  183.  
  184. #if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
  185. void CNavMesh::TileContainerArray::BreakOnInvalidTriangle(const TriangleID triangleID) const
  186. {
  187.         const TileID checkTileID = ComputeTileID(triangleID);
  188.         const size_t index = checkTileID - 1;
  189.  
  190.         if (checkTileID == 0)
  191.         {
  192.                 __debugbreak();
  193.         }
  194.         else if (checkTileID > m_tileCapacity)
  195.         {
  196.                 __debugbreak();
  197.         }
  198.         #ifdef TILE_CONTAINER_ARRAY_STRICT_ACCESS_CHECKS
  199.         else if (stl::find(m_freeIndexes, index))
  200.         {
  201.                 __debugbreak();
  202.         }
  203.         else if (index >= m_freeIndexes.size() + m_tileCount)
  204.         {
  205.                 __debugbreak();
  206.         }
  207.         #endif // TILE_CONTAINER_ARRAY_STRICT_ACCESS_CHECKS
  208. }
  209. #endif
  210.  
  211. const real_t CNavMesh::kMinPullingThreshold = real_t(0.05f);
  212. const real_t CNavMesh::kMaxPullingThreshold = real_t(0.95f);
  213. const real_t CNavMesh::kAdjecencyCalculationToleranceSq = square(real_t(0.02f));
  214.  
  215. CNavMesh::CNavMesh()
  216.         : m_triangleCount(0)
  217. {
  218.         m_islands.reserve(32);
  219. }
  220.  
  221. CNavMesh::~CNavMesh()
  222. {
  223. }
  224.  
  225. void CNavMesh::Init(const SGridParams& params)
  226. {
  227.         m_params = params;
  228.  
  229.         m_tiles.Init(params.tileCount);
  230. }
  231.  
  232. //! Filter to support old GetTriangles() function (and such), which allow to specify minIslandArea
  233. struct CNavMesh::SMinIslandAreaQueryTrianglesFilter
  234. {
  235.         SMinIslandAreaQueryTrianglesFilter(const CNavMesh& navMesh_, float minIslandArea_)
  236.                 : navMesh(navMesh_)
  237.                 , minIslandArea(minIslandArea_)
  238.         {}
  239.  
  240.         bool                                    IsAcceptAll() const { return minIslandArea <= 0.f; }
  241.  
  242.         NavMesh::IQueryTrianglesFilter::EResult Check(const TriangleID triangleId) const
  243.         {
  244.                 return
  245.                   (navMesh.GetIslandAreaForTriangle(triangleId) >= minIslandArea)
  246.                   ? NavMesh::IQueryTrianglesFilter::EResult::Accepted
  247.                   : NavMesh::IQueryTrianglesFilter::EResult::Rejected;
  248.         }
  249.  
  250.         const CNavMesh& navMesh;
  251.         float           minIslandArea;
  252. };
  253.  
  254. template<typename TFilter>
  255. size_t CNavMesh::QueryTileTrianglesLinear(const TileID tileID, const STile& tile, const aabb_t& queryAabbTile, TFilter& filter, const size_t maxTrianglesCount, TriangleID* pOutTriangles) const
  256. {
  257.         CRY_ASSERT(maxTrianglesCount > 0);
  258.         if (maxTrianglesCount == 0)
  259.                 return 0;
  260.  
  261.         size_t triCount = 0;
  262.         for (size_t i = 0; i < tile.triangleCount; ++i)
  263.         {
  264.                 const Tile::STriangle& triangle = tile.triangles[i];
  265.  
  266.                 const Tile::Vertex& v0 = tile.vertices[triangle.vertex[0]];
  267.                 const Tile::Vertex& v1 = tile.vertices[triangle.vertex[1]];
  268.                 const Tile::Vertex& v2 = tile.vertices[triangle.vertex[2]];
  269.  
  270.                 const aabb_t triaabb(vector3_t::minimize(v0, v1, v2), vector3_t::maximize(v0, v1, v2));
  271.  
  272.                 if (queryAabbTile.overlaps(triaabb))
  273.                 {
  274.                         const TriangleID triangleID = ComputeTriangleID(tileID, static_cast<uint16>(i));
  275.  
  276.                         if (filter.Check(triangleID) == NavMesh::IQueryTrianglesFilter::EResult::Accepted)
  277.                         {
  278.                                 pOutTriangles[triCount++] = triangleID;
  279.  
  280.                                 if (triCount == maxTrianglesCount)
  281.                                         break;
  282.                         }
  283.                 }
  284.         }
  285.         return triCount;
  286. }
  287.  
  288. template<typename TFilter>
  289. size_t CNavMesh::QueryTileTrianglesBV(const TileID tileID, const STile& tile, const aabb_t& queryAabbTile, TFilter& filter, const size_t maxTrianglesCount, TriangleID* pOutTriangles) const
  290. {
  291.         CRY_ASSERT(maxTrianglesCount > 0);
  292.         if (maxTrianglesCount == 0)
  293.                 return 0;
  294.  
  295.         size_t triCount = 0;
  296.  
  297.         const size_t nodeCount = tile.nodeCount;
  298.         size_t nodeID = 0;
  299.         while (nodeID < nodeCount)
  300.         {
  301.                 const Tile::SBVNode& node = tile.nodes[nodeID];
  302.  
  303.                 if (!queryAabbTile.overlaps(node.aabb))
  304.                         nodeID += node.leaf ? 1 : node.offset;
  305.                 else
  306.                 {
  307.                         ++nodeID;
  308.  
  309.                         if (node.leaf)
  310.                         {
  311.                                 const uint16 triangleIdx = node.offset;
  312.                                 const TriangleID triangleID = ComputeTriangleID(tileID, triangleIdx);
  313.  
  314.                                 if (filter.Check(triangleID) == NavMesh::IQueryTrianglesFilter::EResult::Accepted)
  315.                                 {
  316.                                         pOutTriangles[triCount++] = triangleID;
  317.  
  318.                                         if (triCount == maxTrianglesCount)
  319.                                                 break;
  320.                                 }
  321.                         }
  322.                 }
  323.         }
  324.         return triCount;
  325. }
  326.  
  327. template<typename TFilter>
  328. size_t CNavMesh::QueryTileTriangles(const TileID tileID, const vector3_t& tileOrigin, const aabb_t& queryAabbWorld, TFilter& filter, const size_t maxTrianglesCount, TriangleID* pOutTriangles) const
  329. {
  330.         const STile& tile = GetTile(tileID);
  331.  
  332.         const vector3_t tileMin(0, 0, 0);
  333.         const vector3_t tileMax(m_params.tileSize.x, m_params.tileSize.y, m_params.tileSize.z);
  334.  
  335.         aabb_t queryAabbTile(queryAabbWorld);
  336.         queryAabbTile.min = vector3_t::maximize(queryAabbTile.min - tileOrigin, tileMin);
  337.         queryAabbTile.max = vector3_t::minimize(queryAabbTile.max - tileOrigin, tileMax);
  338.  
  339.         if (!tile.nodeCount)
  340.         {
  341.                 return QueryTileTrianglesLinear(tileID, tile, queryAabbTile, filter, maxTrianglesCount, pOutTriangles);
  342.         }
  343.         else
  344.         {
  345.                 return QueryTileTrianglesBV(tileID, tile, queryAabbTile, filter, maxTrianglesCount, pOutTriangles);
  346.         }
  347. }
  348.  
  349. template<typename TFilter>
  350. size_t CNavMesh::QueryTrianglesWithFilterInternal(const aabb_t& queryAabbWorld, TFilter& filter, const size_t maxTrianglesCount, TriangleID* pOutTriangles) const
  351. {
  352.         CRY_ASSERT(pOutTriangles);
  353.         CRY_ASSERT(maxTrianglesCount > 0);
  354.         if (!(pOutTriangles && maxTrianglesCount > 0))
  355.         {
  356.                 return 0;
  357.         }
  358.  
  359.         const size_t minX = (std::max(queryAabbWorld.min.x, real_t(0)) / real_t(m_params.tileSize.x)).as_uint();
  360.         const size_t minY = (std::max(queryAabbWorld.min.y, real_t(0)) / real_t(m_params.tileSize.y)).as_uint();
  361.         const size_t minZ = (std::max(queryAabbWorld.min.z, real_t(0)) / real_t(m_params.tileSize.z)).as_uint();
  362.  
  363.         const size_t maxX = (std::max(queryAabbWorld.max.x, real_t(0)) / real_t(m_params.tileSize.x)).as_uint();
  364.         const size_t maxY = (std::max(queryAabbWorld.max.y, real_t(0)) / real_t(m_params.tileSize.y)).as_uint();
  365.         const size_t maxZ = (std::max(queryAabbWorld.max.z, real_t(0)) / real_t(m_params.tileSize.z)).as_uint();
  366.  
  367.         size_t triCount = 0;
  368.  
  369.         TriangleID* pTrianglesBegin = pOutTriangles;
  370.         size_t maxTrianglesCountLeft = maxTrianglesCount;
  371.  
  372.         for (uint y = minY; y <= maxY; ++y)
  373.         {
  374.                 for (uint x = minX; x <= maxX; ++x)
  375.                 {
  376.                         for (uint z = minZ; z <= maxZ; ++z)
  377.                         {
  378.                                 if (const TileID tileID = GetTileID(x, y, z))
  379.                                 {
  380.                                         const vector3_t tileOrigin(
  381.                                           real_t(x * m_params.tileSize.x),
  382.                                           real_t(y * m_params.tileSize.y),
  383.                                           real_t(z * m_params.tileSize.z));
  384.  
  385.                                         const size_t trianglesInTileCount = QueryTileTriangles(tileID, tileOrigin, queryAabbWorld, filter, maxTrianglesCountLeft, pTrianglesBegin);
  386.  
  387.                                         CRY_ASSERT(maxTrianglesCountLeft >= trianglesInTileCount);
  388.                                         maxTrianglesCountLeft -= trianglesInTileCount;
  389.                                         pTrianglesBegin += trianglesInTileCount;
  390.                                         triCount += trianglesInTileCount;
  391.  
  392.                                         if (maxTrianglesCountLeft == 0)
  393.                                         {
  394.                                                 return triCount;
  395.                                         }
  396.                                 }
  397.                         }
  398.                 }
  399.         }
  400.  
  401.         return triCount;
  402. }
  403.  
  404. size_t CNavMesh::QueryTrianglesNoFilterInternal(const aabb_t& queryAabbWorld, const size_t maxTrianglesCount, TriangleID* pOutTriangles) const
  405. {
  406.         SAcceptAllQueryTrianglesFilter filter;
  407.         return QueryTrianglesWithFilterInternal(queryAabbWorld, filter, maxTrianglesCount, pOutTriangles);
  408. }
  409.  
  410. TriangleID CNavMesh::FindClosestTriangleInternal(
  411.   const vector3_t& queryPosWorld,
  412.   const TriangleID* pCandidateTriangles,
  413.   const size_t candidateTrianglesCount,
  414.   vector3_t* pOutClosestPosWorld,
  415.   real_t::unsigned_overflow_type* pOutClosestDistanceSq) const
  416. {
  417.         TriangleID closestID = Constants::InvalidTriangleID;
  418.         MNM::real_t::unsigned_overflow_type closestDistanceSq = std::numeric_limits<MNM::real_t::unsigned_overflow_type>::max();
  419.         vector3_t closestPos(real_t::max());
  420.  
  421.         if (candidateTrianglesCount)
  422.         {
  423.                 MNM::vector3_t a, b, c;
  424.  
  425.                 for (size_t i = 0; i < candidateTrianglesCount; ++i)
  426.                 {
  427.                         const TriangleID triangleId = pCandidateTriangles[i];
  428.  
  429.                         if (GetVertices(triangleId, a, b, c))
  430.                         {
  431.                                 const MNM::vector3_t ptClosest = ClosestPtPointTriangle(queryPosWorld, a, b, c);
  432.                                 const MNM::real_t::unsigned_overflow_type dSq = (ptClosest - queryPosWorld).lenSqNoOverflow();
  433.  
  434.                                 if (dSq < closestDistanceSq)
  435.                                 {
  436.                                         closestID = triangleId;
  437.                                         closestDistanceSq = dSq;
  438.                                         closestPos = ptClosest;
  439.                                 }
  440.                         }
  441.                 }
  442.         }
  443.  
  444.         if (closestID != Constants::InvalidTriangleID)
  445.         {
  446.                 if (pOutClosestPosWorld)
  447.                 {
  448.                         *pOutClosestPosWorld = closestPos;
  449.                 }
  450.  
  451.                 if (pOutClosestDistanceSq)
  452.                 {
  453.                         *pOutClosestDistanceSq = closestDistanceSq;
  454.                 }
  455.         }
  456.  
  457.         return closestID;
  458. }
  459.  
  460. #pragma warning(push)
  461. #pragma warning(disable:28285)
  462. size_t CNavMesh::GetTriangles(aabb_t aabb, TriangleID* triangles, size_t maxTriCount, float minIslandArea) const
  463. {
  464.         SMinIslandAreaQueryTrianglesFilter filter(*this, minIslandArea);
  465.         if (filter.IsAcceptAll())
  466.         {
  467.                 return QueryTrianglesNoFilterInternal(aabb, maxTriCount, triangles);
  468.         }
  469.         else
  470.         {
  471.                 return QueryTrianglesWithFilterInternal(aabb, filter, maxTriCount, triangles);
  472.         }
  473. }
  474. #pragma warning(pop)
  475.  
  476. TriangleID CNavMesh::GetTriangleAt(const vector3_t& location, const real_t verticalDownwardRange, const real_t verticalUpwardRange, float minIslandArea) const
  477. {
  478.         const MNM::aabb_t aabb(
  479.           MNM::vector3_t(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z - verticalDownwardRange)),
  480.           MNM::vector3_t(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z + verticalUpwardRange)));
  481.  
  482.         const size_t MaxTriCandidateCount = 1024;
  483.         TriangleID candidates[MaxTriCandidateCount];
  484.  
  485.         TriangleID closestID = 0;
  486.  
  487.         const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount, minIslandArea);
  488.         MNM::real_t::unsigned_overflow_type distMinSq = std::numeric_limits<MNM::real_t::unsigned_overflow_type>::max();
  489.  
  490.         if (candidateCount)
  491.         {
  492.                 MNM::vector3_t a, b, c;
  493.  
  494.                 for (size_t i = 0; i < candidateCount; ++i)
  495.                 {
  496.                         GetVertices(candidates[i], a, b, c);
  497.  
  498.                         if (PointInTriangle(vector2_t(location), vector2_t(a), vector2_t(b), vector2_t(c)))
  499.                         {
  500.                                 const MNM::vector3_t ptClosest = ClosestPtPointTriangle(location, a, b, c);
  501.                                 const MNM::real_t::unsigned_overflow_type dSq = (ptClosest - location).lenSqNoOverflow();
  502.  
  503.                                 if (dSq < distMinSq)
  504.                                 {
  505.                                         distMinSq = dSq;
  506.                                         closestID = candidates[i];
  507.                                 }
  508.                         }
  509.                 }
  510.         }
  511.  
  512.         return closestID;
  513. }
  514.  
  515. TriangleID CNavMesh::GetTriangleEdgeAlongLine(const vector3_t& startLocation, const vector3_t& endLocation,
  516.                                               const real_t verticalDownwardRange, const real_t verticalUpwardRange, vector3_t& hit, float minIslandArea) const
  517. {
  518.         // Determine bounding box
  519.         real_t minX, minY, minZ;
  520.         real_t maxX = (startLocation.x > endLocation.x) ? (minX = endLocation.x, startLocation.x) : (minX = startLocation.x, endLocation.x);
  521.         real_t maxY = (startLocation.y > endLocation.y) ? (minY = endLocation.y, startLocation.y) : (minY = startLocation.y, endLocation.y);
  522.         real_t maxZ = (startLocation.z > endLocation.z) ? (minZ = endLocation.z, startLocation.z) : (minZ = startLocation.z, endLocation.z);
  523.  
  524.         const aabb_t aabb(vector3_t(minX, minY, minZ - verticalDownwardRange), vector3_t(maxX, maxY, maxZ + verticalUpwardRange));
  525.  
  526.         // Gather intersecting triangles
  527.         const size_t MaxTriCandidateCount = 1024;
  528.         TriangleID candidates[MaxTriCandidateCount];
  529.         const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount /*, minIslandArea parameter is no longer supported*/);
  530.  
  531.         TriangleID triangleID = 0;
  532.  
  533.         if (candidateCount)
  534.         {
  535.                 vector3_t verts[3];
  536.                 real_t s, t;
  537.                 real_t closest = real_t::max();
  538.  
  539.                 for (size_t i = 0; i < candidateCount; ++i)
  540.                 {
  541.                         if (GetVertices(candidates[i], verts))
  542.                         {
  543.                                 for (uint8 v = 0; v < 3; ++v)
  544.                                 {
  545.                                         if (IntersectSegmentSegment(startLocation, endLocation, verts[v], verts[next_mod3(v)], s, t))
  546.                                         {
  547.                                                 if (s < closest)
  548.                                                 {
  549.                                                         closest = s;
  550.                                                         vector3_t d = (endLocation - startLocation);
  551.                                                         hit = startLocation + (d * s);
  552.                                                         triangleID = candidates[i];
  553.                                                 }
  554.                                         }
  555.                                 }
  556.                         }
  557.                 }
  558.         }
  559.  
  560.         return triangleID;
  561. }
  562.  
  563. bool CNavMesh::IsTriangleAcceptableForLocation(const vector3_t& location, TriangleID triangleID) const
  564. {
  565.         const MNM::real_t range = MNM::real_t(1.0f);
  566.         if (triangleID)
  567.         {
  568.                 const MNM::aabb_t aabb(
  569.                   MNM::vector3_t(MNM::real_t(location.x - range), MNM::real_t(location.y - range), MNM::real_t(location.z - range)),
  570.                   MNM::vector3_t(MNM::real_t(location.x + range), MNM::real_t(location.y + range), MNM::real_t(location.z + range)));
  571.  
  572.                 const size_t MaxTriCandidateCount = 1024;
  573.                 TriangleID candidates[MaxTriCandidateCount];
  574.  
  575.                 const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount);
  576.                 MNM::real_t distMinSq = MNM::real_t::max();
  577.  
  578.                 if (candidateCount)
  579.                 {
  580.                         MNM::vector3_t start = location;
  581.                         MNM::vector3_t a, b, c;
  582.  
  583.                         for (size_t i = 0; i < candidateCount; ++i)
  584.                         {
  585.                                 GetVertices(candidates[i], a, b, c);
  586.  
  587.                                 if (candidates[i] == triangleID && PointInTriangle(vector2_t(location), vector2_t(a), vector2_t(b), vector2_t(c)))
  588.                                 {
  589.                                         return true;
  590.                                 }
  591.                         }
  592.                 }
  593.         }
  594.  
  595.         return false;
  596. }
  597.  
  598. TriangleID CNavMesh::GetClosestTriangle(const vector3_t& location, real_t vrange, real_t hrange, real_t* distance,
  599.                                         vector3_t* closest, float minIslandArea) const
  600. {
  601.         const MNM::aabb_t aabb(
  602.           MNM::vector3_t(MNM::real_t(location.x - hrange), MNM::real_t(location.y - hrange), MNM::real_t(location.z - vrange)),
  603.           MNM::vector3_t(MNM::real_t(location.x + hrange), MNM::real_t(location.y + hrange), MNM::real_t(location.z + vrange)));
  604.  
  605.         const size_t MaxTriCandidateCount = 1024;
  606.         TriangleID candidates[MaxTriCandidateCount];
  607.  
  608.         const size_t candidatesCount = GetTriangles(aabb, candidates, MaxTriCandidateCount, minIslandArea);
  609.         real_t::unsigned_overflow_type distanceSq;
  610.         const TriangleID resultClosestTriangleId = FindClosestTriangleInternal(location, candidates, candidatesCount, closest, &distanceSq);
  611.  
  612.         if ((resultClosestTriangleId != Constants::InvalidTriangleID) && (distance != nullptr))
  613.         {
  614.                 *distance = real_t::sqrtf(distanceSq);
  615.         }
  616.  
  617.         return resultClosestTriangleId;
  618. }
  619.  
  620. bool CNavMesh::GetVertices(TriangleID triangleID, vector3_t& v0, vector3_t& v1, vector3_t& v2) const
  621. {
  622.         if (const TileID tileID = ComputeTileID(triangleID))
  623.         {
  624.                 const TileContainer& container = m_tiles[tileID - 1];
  625.  
  626.                 const vector3_t origin(
  627.                   real_t(container.x * m_params.tileSize.x),
  628.                   real_t(container.y * m_params.tileSize.y),
  629.                   real_t(container.z * m_params.tileSize.z));
  630.  
  631.                 const Tile::STriangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
  632.                 v0 = origin + vector3_t(container.tile.vertices[triangle.vertex[0]]);
  633.                 v1 = origin + vector3_t(container.tile.vertices[triangle.vertex[1]]);
  634.                 v2 = origin + vector3_t(container.tile.vertices[triangle.vertex[2]]);
  635.  
  636.                 return true;
  637.         }
  638.  
  639.         return false;
  640. }
  641.  
  642. bool CNavMesh::GetVertices(TriangleID triangleID, vector3_t* verts) const
  643. {
  644.         return GetVertices(triangleID, verts[0], verts[1], verts[2]);
  645. }
  646.  
  647. bool CNavMesh::GetLinkedEdges(TriangleID triangleID, size_t& linkedEdges) const
  648. {
  649.         if (const TileID tileID = ComputeTileID(triangleID))
  650.         {
  651.                 const TileContainer& container = m_tiles[tileID - 1];
  652.                 const Tile::STriangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
  653.  
  654.                 linkedEdges = 0;
  655.  
  656.                 for (size_t l = 0; l < triangle.linkCount; ++l)
  657.                 {
  658.                         const Tile::SLink& link = container.tile.links[triangle.firstLink + l];
  659.                         const size_t edge = link.edge;
  660.                         linkedEdges |= static_cast<size_t>(1) << edge;
  661.                 }
  662.  
  663.                 return true;
  664.         }
  665.  
  666.         return false;
  667. }
  668.  
  669. bool CNavMesh::GetTriangle(TriangleID triangleID, Tile::STriangle& triangle) const
  670. {
  671.         if (const TileID tileID = ComputeTileID(triangleID))
  672.         {
  673.                 const TileContainer& container = m_tiles[tileID - 1];
  674.                 triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
  675.  
  676.                 return true;
  677.         }
  678.  
  679.         return false;
  680. }
  681.  
  682. bool CNavMesh::PushPointInsideTriangle(const TriangleID triangleID, vector3_t& location, real_t amount) const
  683. {
  684.         if (amount <= 0)
  685.                 return false;
  686.  
  687.         vector3_t v0, v1, v2;
  688.         if (const TileID tileID = ComputeTileID(triangleID))
  689.         {
  690.                 const TileContainer& container = m_tiles[tileID - 1];
  691.                 const vector3_t origin(
  692.                   real_t(container.x * m_params.tileSize.x),
  693.                   real_t(container.y * m_params.tileSize.y),
  694.                   real_t(container.z * m_params.tileSize.z));
  695.                 const vector3_t locationTileOffsetted = (location - origin);
  696.  
  697.                 const Tile::STriangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)];
  698.                 v0 = vector3_t(container.tile.vertices[triangle.vertex[0]]);
  699.                 v1 = vector3_t(container.tile.vertices[triangle.vertex[1]]);
  700.                 v2 = vector3_t(container.tile.vertices[triangle.vertex[2]]);
  701.  
  702.                 const vector3_t triangleCenter = ((v0 + v1 + v2) * real_t::fraction(1, 3));
  703.                 const vector3_t locationToCenter = triangleCenter - locationTileOffsetted;
  704.                 const real_t locationToCenterLen = locationToCenter.lenNoOverflow();
  705.  
  706.                 if (locationToCenterLen > amount)
  707.                 {
  708.                         location += (locationToCenter / locationToCenterLen) * amount;
  709.                 }
  710.                 else
  711.                 {
  712.                         // If locationToCenterLen is smaller than the amount I wanna push
  713.                         // the point, then it's safer use directly the center position
  714.                         // otherwise the point could end up outside the other side of the triangle
  715.                         location = triangleCenter + origin;
  716.                 }
  717.                 return true;
  718.         }
  719.  
  720.         return false;
  721. }
  722.  
  723. void CNavMesh::ResetConnectedIslandsIDs()
  724. {
  725.         for (TileMap::iterator tileIt = m_tileMap.begin(); tileIt != m_tileMap.end(); ++tileIt)
  726.         {
  727.                 STile& tile = m_tiles[tileIt->second - 1].tile;
  728.  
  729.                 for (uint16 i = 0; i < tile.triangleCount; ++i)
  730.                 {
  731.                         Tile::STriangle& triangle = tile.triangles[i];
  732.                         triangle.islandID = MNM::Constants::eStaticIsland_InvalidIslandID;
  733.                 }
  734.         }
  735.  
  736.         m_islands.clear();
  737. }
  738.  
  739. void CNavMesh::ComputeStaticIslandsAndConnections(const NavigationMeshID meshID, const OffMeshNavigationManager& offMeshNavigationManager, MNM::IslandConnections& islandConnections)
  740. {
  741.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  742.  
  743.         ResetConnectedIslandsIDs();
  744.         ComputeStaticIslands();
  745.         ResolvePendingIslandConnectionRequests(meshID, offMeshNavigationManager, islandConnections);
  746. }
  747.  
  748. void CNavMesh::ComputeStaticIslands()
  749. {
  750.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  751.  
  752.         typedef std::vector<TriangleID> Triangles;
  753.         Triangles trianglesToVisit;
  754.         trianglesToVisit.reserve(4096);
  755.  
  756.         // The idea is to iterate through the existing tiles and then finding the triangles from which
  757.         // start the assigning of the different island ids.
  758.         for (TileMap::iterator tileIt = m_tileMap.begin(); tileIt != m_tileMap.end(); ++tileIt)
  759.         {
  760.                 const TileID tileID = tileIt->second;
  761.                 STile& tile = m_tiles[tileID - 1].tile;
  762.                 for (uint16 triangleIndex = 0; triangleIndex < tile.triangleCount; ++triangleIndex)
  763.                 {
  764.                         Tile::STriangle& sourceTriangle = tile.triangles[triangleIndex];
  765.                         if (sourceTriangle.islandID == MNM::Constants::eStaticIsland_InvalidIslandID)
  766.                         {
  767.                                 Island& newIsland = GetNewIsland();
  768.                                 sourceTriangle.islandID = newIsland.id;
  769.                                 const TriangleID triangleID = ComputeTriangleID(tileID, triangleIndex);
  770.  
  771.                                 newIsland.area = tile.GetTriangleArea(triangleID).as_float();
  772.  
  773.                                 trianglesToVisit.push_back(triangleID);
  774.  
  775.                                 // We now have another triangle to start from to assign the new island ids to all the connected
  776.                                 // triangles
  777.                                 size_t totalTrianglesToVisit = 1;
  778.                                 for (size_t index = 0; index < totalTrianglesToVisit; ++index)
  779.                                 {
  780.                                         // Get next triangle to start the evaluation from
  781.                                         const MNM::TriangleID currentTriangleID = trianglesToVisit[index];
  782.  
  783.                                         const TileID currentTileId = ComputeTileID(currentTriangleID);
  784.                                         CRY_ASSERT_MESSAGE(currentTileId > 0, "ComputeStaticIslands is trying to access a triangle ID associated with an invalid tile id.");
  785.  
  786.                                         const TileContainer& container = m_tiles[currentTileId - 1];
  787.                                         const STile& currentTile = container.tile;
  788.                                         const Tile::STriangle& currentTriangle = currentTile.triangles[ComputeTriangleIndex(currentTriangleID)];
  789.  
  790.                                         // Calc area of this triangle
  791.                                         float farea = currentTile.GetTriangleArea(currentTriangleID).as_float();
  792.  
  793.                                         for (uint16 l = 0; l < currentTriangle.linkCount; ++l)
  794.                                         {
  795.                                                 const Tile::SLink& link = currentTile.links[currentTriangle.firstLink + l];
  796.                                                 if (link.side == Tile::SLink::Internal)
  797.                                                 {
  798.                                                         Tile::STriangle& nextTriangle = currentTile.triangles[link.triangle];
  799.                                                         if (nextTriangle.islandID == MNM::Constants::eGlobalIsland_InvalidIslandID)
  800.                                                         {
  801.                                                                 ++totalTrianglesToVisit;
  802.                                                                 nextTriangle.islandID = newIsland.id;
  803.                                                                 newIsland.area += farea;
  804.                                                                 trianglesToVisit.push_back(ComputeTriangleID(currentTileId, link.triangle));
  805.                                                         }
  806.                                                 }
  807.                                                 else if (link.side == Tile::SLink::OffMesh)
  808.                                                 {
  809.                                                         QueueIslandConnectionSetup(currentTriangle.islandID, currentTriangleID, link.triangle);
  810.                                                 }
  811.                                                 else
  812.                                                 {
  813.                                                         const TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side);
  814.                                                         CRY_ASSERT_MESSAGE(neighbourTileID > 0, "ComputeStaticIslands is trying to access an invalid neighbour tile.");
  815.  
  816.                                                         const STile& neighbourTile = m_tiles[neighbourTileID - 1].tile;
  817.                                                         Tile::STriangle& nextTriangle = neighbourTile.triangles[link.triangle];
  818.                                                         if (nextTriangle.islandID == MNM::Constants::eGlobalIsland_InvalidIslandID)
  819.                                                         {
  820.                                                                 ++totalTrianglesToVisit;
  821.                                                                 nextTriangle.islandID = newIsland.id;
  822.                                                                 newIsland.area += farea;
  823.                                                                 trianglesToVisit.push_back(ComputeTriangleID(neighbourTileID, link.triangle));
  824.                                                         }
  825.                                                 }
  826.                                         }
  827.                                 }
  828.                                 trianglesToVisit.clear();
  829.                         }
  830.                 }
  831.         }
  832. }
  833.  
  834. CNavMesh::Island& CNavMesh::GetNewIsland()
  835. {
  836.         assert(m_islands.size() != std::numeric_limits<StaticIslandID>::max());
  837.  
  838.         // Generate new id (NOTE: Invalid is 0)
  839.         StaticIslandID id = (m_islands.size() + 1);
  840.  
  841.         m_islands.push_back(Island(id));
  842.         return m_islands.back();
  843. }
  844.  
  845. void CNavMesh::QueueIslandConnectionSetup(const StaticIslandID islandID, const TriangleID startingTriangleID, const uint16 offMeshLinkIndex)
  846. {
  847.         m_islandConnectionRequests.push_back(IslandConnectionRequest(islandID, startingTriangleID, offMeshLinkIndex));
  848. }
  849.  
  850. void CNavMesh::ResolvePendingIslandConnectionRequests(const NavigationMeshID meshID, const OffMeshNavigationManager& offMeshNavigationManager,
  851.                                                       MNM::IslandConnections& islandConnections)
  852. {
  853.         const OffMeshNavigation& offMeshNavigation = offMeshNavigationManager.GetOffMeshNavigationForMesh(meshID);
  854.  
  855.         while (!m_islandConnectionRequests.empty())
  856.         {
  857.                 IslandConnectionRequest& request = m_islandConnectionRequests.back();
  858.                 OffMeshNavigation::QueryLinksResult links = offMeshNavigation.GetLinksForTriangle(request.startingTriangleID, request.offMeshLinkIndex);
  859.                 while (WayTriangleData nextTri = links.GetNextTriangle())
  860.                 {
  861.                         Tile::STriangle endTriangle;
  862.                         if (GetTriangle(nextTri.triangleID, endTriangle))
  863.                         {
  864.                                 const OffMeshLink* pLink = offMeshNavigationManager.GetOffMeshLink(nextTri.offMeshLinkID);
  865.                                 assert(pLink);
  866.                                 MNM::IslandConnections::Link link(nextTri.triangleID, nextTri.offMeshLinkID, GlobalIslandID(meshID, endTriangle.islandID), pLink->GetEntityIdForOffMeshLink());
  867.                                 islandConnections.SetOneWayConnectionBetweenIsland(GlobalIslandID(meshID, request.startingIslandID), link);
  868.                         }
  869.                 }
  870.                 m_islandConnectionRequests.pop_back();
  871.         }
  872. }
  873.  
  874. void CNavMesh::SearchForIslandConnectionsToRefresh(const TileID tileID)
  875. {
  876.         STile& tile = m_tiles[tileID - 1].tile;
  877.         for (uint16 triangleIndex = 0; triangleIndex < tile.triangleCount; ++triangleIndex)
  878.         {
  879.                 Tile::STriangle& triangle = tile.triangles[triangleIndex];
  880.                 for (uint16 l = 0; l < triangle.linkCount; ++l)
  881.                 {
  882.                         const Tile::SLink& link = tile.links[triangle.firstLink + l];
  883.                         if (link.side == Tile::SLink::OffMesh)
  884.                         {
  885.                                 QueueIslandConnectionSetup(triangle.islandID, ComputeTriangleID(tileID, triangleIndex), link.triangle);
  886.                         }
  887.                 }
  888.         }
  889. }
  890.  
  891. float CNavMesh::GetIslandArea(StaticIslandID islandID) const
  892. {
  893.         bool isValid = (islandID >= MNM::Constants::eStaticIsland_FirstValidIslandID && islandID <= m_islands.size());
  894.         return (isValid) ? m_islands[islandID - 1].area : -1.f;
  895. }
  896.  
  897. float CNavMesh::GetIslandAreaForTriangle(TriangleID triangleID) const
  898. {
  899.         Tile::STriangle triangle;
  900.         if (GetTriangle(triangleID, triangle))
  901.         {
  902.                 return GetIslandArea(triangle.islandID);
  903.         }
  904.  
  905.         return -1.f;
  906. }
  907.  
  908. void CNavMesh::PredictNextTriangleEntryPosition(const TriangleID bestNodeTriangleID,
  909.                                                 const vector3_t& bestNodePosition, const TriangleID nextTriangleID, const unsigned int edgeIndex
  910.                                                 , const vector3_t& finalLocation, vector3_t& outPosition) const
  911. {
  912.         IF_UNLIKELY (edgeIndex == MNM::Constants::InvalidEdgeIndex)
  913.         {
  914.                 // For SO links we don't set up the edgeIndex value since it's more probable that the animations
  915.                 // ending point is better approximated by the triangle center value
  916.                 vector3_t v0, v1, v2;
  917.                 GetVertices(nextTriangleID, v0, v1, v2);
  918.                 outPosition = (v0 + v1 + v2) * real_t::fraction(1, 3);
  919.                 return;
  920.         }
  921.  
  922.         Tile::STriangle triangle;
  923.         if (GetTriangle(bestNodeTriangleID, triangle))
  924.         {
  925.                 const TileID bestTriangleTileID = ComputeTileID(bestNodeTriangleID);
  926.                 assert(bestTriangleTileID);
  927.                 const TileContainer& currentContainer = m_tiles[bestTriangleTileID - 1];
  928.                 const STile& currentTile = currentContainer.tile;
  929.                 const vector3_t tileOrigin(real_t(currentContainer.x * m_params.tileSize.x), real_t(currentContainer.y * m_params.tileSize.y), real_t(currentContainer.z * m_params.tileSize.z));
  930.  
  931.                 assert(edgeIndex < 3);
  932.                 const vector3_t v0 = tileOrigin + vector3_t(currentTile.vertices[triangle.vertex[edgeIndex]]);
  933.                 const vector3_t v1 = tileOrigin + vector3_t(currentTile.vertices[triangle.vertex[next_mod3(edgeIndex)]]);
  934.  
  935.                 switch (gAIEnv.CVars.MNMPathfinderPositionInTrianglePredictionType)
  936.                 {
  937.                 case ePredictionType_TriangleCenter:
  938.                         {
  939.                                 const vector3_t v2 = tileOrigin + vector3_t(currentTile.vertices[triangle.vertex[dec_mod3[edgeIndex]]]);
  940.                                 outPosition = (v0 + v1 + v2) * real_t::fraction(1, 3);
  941.                         }
  942.                         break;
  943.                 case ePredictionType_Advanced:
  944.                 default:
  945.                         {
  946.                                 const vector3_t v0v1 = v1 - v0;
  947.                                 real_t s, t;
  948.                                 if (IntersectSegmentSegment(v0, v1, bestNodePosition, finalLocation, s, t))
  949.                                 {
  950.                                         // If the two segments intersect,
  951.                                         // let's choose the point that goes in the direction we want to go
  952.                                         s = clamp(s, kMinPullingThreshold, kMaxPullingThreshold);
  953.                                         outPosition = v0 + v0v1 * s;
  954.                                 }
  955.                                 else
  956.                                 {
  957.                                         // Otherwise we need to understand where the segment is in relation
  958.                                         // of where we want to go.
  959.                                         // Let's choose the point on the segment that is closer towards the target
  960.                                         const real_t::unsigned_overflow_type distSqAE = (v0 - finalLocation).lenSqNoOverflow();
  961.                                         const real_t::unsigned_overflow_type distSqBE = (v1 - finalLocation).lenSqNoOverflow();
  962.                                         const real_t segmentPercentage = (distSqAE < distSqBE) ? kMinPullingThreshold : kMaxPullingThreshold;
  963.                                         outPosition = v0 + v0v1 * segmentPercentage;
  964.                                 }
  965.                         }
  966.                         break;
  967.                 }
  968.         }
  969.         else
  970.         {
  971.                 // At this point it's not acceptable to have an invalid triangle
  972.                 assert(0);
  973.         }
  974. }
  975.  
  976. CNavMesh::EWayQueryResult CNavMesh::FindWay(WayQueryRequest& inputRequest, WayQueryWorkingSet& workingSet, WayQueryResult& result) const
  977. {
  978.         result.SetWaySize(0);
  979.         if (result.GetWayMaxSize() < 2)
  980.         {
  981.                 return eWQR_Done;
  982.         }
  983.  
  984.         if (inputRequest.From() && inputRequest.To())
  985.         {
  986.                 if (inputRequest.From() == inputRequest.To())
  987.                 {
  988.                         WayTriangleData* pOutputWay = result.GetWayData();
  989.                         pOutputWay[0] = WayTriangleData(inputRequest.From(), 0);
  990.                         pOutputWay[1] = WayTriangleData(inputRequest.To(), 0);
  991.                         result.SetWaySize(2);
  992.                         return eWQR_Done;
  993.                 }
  994.                 else
  995.                 {
  996.                         const vector3_t origin(m_params.origin);
  997.                         const vector3_t startLocation = inputRequest.GetFromLocation();
  998.                         const vector3_t endLocation = inputRequest.GetToLocation();
  999.  
  1000.                         WayTriangleData lastBestNodeID(inputRequest.From(), 0);
  1001.  
  1002.                         while (workingSet.aStarOpenList.CanDoStep())
  1003.                         {
  1004.                                 // switch the smallest element with the last one and pop the last element
  1005.                                 AStarOpenList::OpenNodeListElement element = workingSet.aStarOpenList.PopBestNode();
  1006.                                 WayTriangleData bestNodeID = element.triData;
  1007.  
  1008.                                 lastBestNodeID = bestNodeID;
  1009.                                 IF_UNLIKELY (bestNodeID.triangleID == inputRequest.To())
  1010.                                 {
  1011.                                         workingSet.aStarOpenList.StepDone();
  1012.                                         break;
  1013.                                 }
  1014.  
  1015.                                 AStarOpenList::Node* bestNode = element.pNode;
  1016.                                 const vector3_t bestNodeLocation = bestNode->location - origin;
  1017.  
  1018.                                 const TileID tileID = ComputeTileID(bestNodeID.triangleID);
  1019.  
  1020.                                 // Clear from previous step
  1021.                                 workingSet.nextLinkedTriangles.clear();
  1022.  
  1023.                                 if (tileID)
  1024.                                 {
  1025.                                         const TileContainer& container = m_tiles[tileID - 1];
  1026.                                         const STile& tile = container.tile;
  1027.                                         const uint16 triangleIdx = ComputeTriangleIndex(bestNodeID.triangleID);
  1028.                                         const Tile::STriangle& triangle = tile.triangles[triangleIdx];
  1029.  
  1030.                                         //Gather all accessible triangles first
  1031.  
  1032.                                         for (size_t l = 0; l < triangle.linkCount; ++l)
  1033.                                         {
  1034.                                                 const Tile::SLink& link = tile.links[triangle.firstLink + l];
  1035.  
  1036.                                                 WayTriangleData nextTri(0, 0);
  1037.  
  1038.                                                 if (link.side == Tile::SLink::Internal)
  1039.                                                 {
  1040.                                                         nextTri.triangleID = ComputeTriangleID(tileID, link.triangle);
  1041.                                                         nextTri.incidentEdge = link.edge;
  1042.                                                 }
  1043.                                                 else if (link.side == Tile::SLink::OffMesh)
  1044.                                                 {
  1045.                                                         OffMeshNavigation::QueryLinksResult links = inputRequest.GetOffMeshNavigation().GetLinksForTriangle(bestNodeID.triangleID, link.triangle);
  1046.                                                         while (nextTri = links.GetNextTriangle())
  1047.                                                         {
  1048.                                                                 if (inputRequest.CanUseOffMeshLink(nextTri.offMeshLinkID, &nextTri.costMultiplier))
  1049.                                                                 {
  1050.                                                                         workingSet.nextLinkedTriangles.push_back(nextTri);
  1051.                                                                         nextTri.incidentEdge = (unsigned int)MNM::Constants::InvalidEdgeIndex;
  1052.                                                                 }
  1053.                                                         }
  1054.                                                         continue;
  1055.                                                 }
  1056.                                                 else
  1057.                                                 {
  1058.                                                         TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side);
  1059.                                                         nextTri.triangleID = ComputeTriangleID(neighbourTileID, link.triangle);
  1060.                                                         nextTri.incidentEdge = link.edge;
  1061.                                                 }
  1062.  
  1063.                                                 Vec3 edgeMidPoint;
  1064.                                                 if (CalculateMidEdge(bestNodeID.triangleID, nextTri.triangleID, edgeMidPoint))
  1065.                                                 {
  1066.                                                         const uint32 flags = 0;
  1067.                                                         if (inputRequest.IsPointValidForAgent(edgeMidPoint, flags))
  1068.                                                         {
  1069.                                                                 workingSet.nextLinkedTriangles.push_back(nextTri);
  1070.                                                         }
  1071.                                                 }
  1072.                                                 //////////////////////////////////////////////////////////////////////////
  1073.                                                 // NOTE: This is user defined only at compile time
  1074.  
  1075.                                                 m_tiles.BreakOnInvalidTriangle(nextTri.triangleID);
  1076.  
  1077.                                                 //////////////////////////////////////////////////////////////////////////
  1078.                                         }
  1079.                                 }
  1080.                                 else
  1081.                                 {
  1082.                                         AIError("CNavMesh::FindWay - Bad Navmesh data Tile: %d, Triangle: %d, skipping ", tileID, ComputeTriangleIndex(bestNodeID.triangleID));
  1083.                                         m_tiles.BreakOnInvalidTriangle(bestNodeID.triangleID);
  1084.                                 }
  1085.  
  1086.                                 const size_t triangleCount = workingSet.nextLinkedTriangles.size();
  1087.                                 for (size_t t = 0; t < triangleCount; ++t)
  1088.                                 {
  1089.                                         WayTriangleData nextTri = workingSet.nextLinkedTriangles[t];
  1090.  
  1091.                                         if (nextTri == bestNode->prevTriangle)
  1092.                                                 continue;
  1093.  
  1094.                                         AStarOpenList::Node* nextNode = NULL;
  1095.                                         const bool inserted = workingSet.aStarOpenList.InsertNode(nextTri, &nextNode);
  1096.  
  1097.                                         assert(nextNode);
  1098.  
  1099.                                         if (inserted)
  1100.                                         {
  1101.                                                 IF_UNLIKELY (nextTri.triangleID == inputRequest.To())
  1102.                                                 {
  1103.                                                         nextNode->location = endLocation;
  1104.                                                 }
  1105.                                                 else
  1106.                                                 {
  1107.                                                         PredictNextTriangleEntryPosition(bestNodeID.triangleID, bestNodeLocation, nextTri.triangleID, nextTri.incidentEdge, endLocation, nextNode->location);
  1108.                                                 }
  1109.  
  1110.                                                 nextNode->open = false;
  1111.                                         }
  1112.  
  1113.                                         //Euclidean distance
  1114.                                         const vector3_t targetDistance = endLocation - nextNode->location;
  1115.                                         const vector3_t stepDistance = bestNodeLocation - nextNode->location;
  1116.                                         const real_t heuristic = targetDistance.lenNoOverflow();
  1117.                                         const real_t stepCost = stepDistance.lenNoOverflow();
  1118.  
  1119.                                         //Euclidean distance approximation
  1120.                                         //const real_t heuristic = targetDistance.approximatedLen();
  1121.                                         //const real_t stepCost = stepDistance.approximatedLen();
  1122.  
  1123.                                         const real_t dangersTotalCost = CalculateHeuristicCostForDangers(nextNode->location, startLocation, m_params.origin, inputRequest.GetDangersInfos());
  1124.  
  1125.                                         real_t costMultiplier = real_t(nextTri.costMultiplier);
  1126.  
  1127.                                         const real_t cost = bestNode->cost + (stepCost * costMultiplier) + dangersTotalCost;
  1128.                                         const real_t total = cost + heuristic;
  1129.  
  1130.                                         if (nextNode->open && nextNode->estimatedTotalCost <= total)
  1131.                                                 continue;
  1132.  
  1133.                                         nextNode->cost = cost;
  1134.                                         nextNode->estimatedTotalCost = total;
  1135.                                         nextNode->prevTriangle = bestNodeID;
  1136.  
  1137.                                         if (!nextNode->open)
  1138.                                         {
  1139.                                                 nextNode->open = true;
  1140.                                                 nextNode->location += origin;
  1141.                                                 workingSet.aStarOpenList.AddToOpenList(nextTri, nextNode, total);
  1142.                                         }
  1143.                                 }
  1144.  
  1145.                                 workingSet.aStarOpenList.StepDone();
  1146.                         }
  1147.  
  1148.                         if (lastBestNodeID.triangleID == inputRequest.To())
  1149.                         {
  1150.                                 size_t wayTriCount = 0;
  1151.                                 WayTriangleData wayTriangle = lastBestNodeID;
  1152.                                 WayTriangleData nextInsertion(wayTriangle.triangleID, 0);
  1153.  
  1154.                                 WayTriangleData* outputWay = result.GetWayData();
  1155.  
  1156.                                 while (wayTriangle.triangleID != inputRequest.From())
  1157.                                 {
  1158.                                         const AStarOpenList::Node* node = workingSet.aStarOpenList.FindNode(wayTriangle);
  1159.                                         assert(node);
  1160.                                         outputWay[wayTriCount++] = nextInsertion;
  1161.  
  1162.                                         //Iterate the path backwards, and shift the offMeshLink to the previous triangle (start of the link)
  1163.                                         nextInsertion.offMeshLinkID = wayTriangle.offMeshLinkID;
  1164.                                         wayTriangle = node->prevTriangle;
  1165.                                         nextInsertion.triangleID = wayTriangle.triangleID;
  1166.  
  1167.                                         if (wayTriCount == result.GetWayMaxSize())
  1168.                                                 break;
  1169.                                 }
  1170.  
  1171.                                 if (wayTriCount < result.GetWayMaxSize())
  1172.                                 {
  1173.                                         outputWay[wayTriCount++] = WayTriangleData(inputRequest.From(), nextInsertion.offMeshLinkID);
  1174.                                 }
  1175.  
  1176.                                 result.SetWaySize(wayTriCount);
  1177.                                 return eWQR_Done;
  1178.                         }
  1179.                         else if (!workingSet.aStarOpenList.Empty())
  1180.                         {
  1181.                                 //We did not finish yet...
  1182.                                 return eWQR_Continuing;
  1183.                         }
  1184.                 }
  1185.         }
  1186.  
  1187.         return eWQR_Done;
  1188. }
  1189.  
  1190. struct CostAccumulator
  1191. {
  1192.         CostAccumulator(const Vec3& locationToEval, const Vec3& startingLocation, real_t& totalCost)
  1193.                 : m_totalCost(totalCost)
  1194.                 , m_locationToEvaluate(locationToEval)
  1195.                 , m_startingLocation(startingLocation)
  1196.         {}
  1197.  
  1198.         void operator()(const DangerAreaConstPtr& dangerInfo)
  1199.         {
  1200.                 m_totalCost += dangerInfo->GetDangerHeuristicCost(m_locationToEvaluate, m_startingLocation);
  1201.         }
  1202. private:
  1203.         real_t&     m_totalCost;
  1204.         const Vec3& m_locationToEvaluate;
  1205.         const Vec3& m_startingLocation;
  1206. };
  1207.  
  1208. real_t CNavMesh::CalculateHeuristicCostForDangers(const vector3_t& locationToEval, const vector3_t& startingLocation, const Vec3& meshOrigin, const DangerousAreasList& dangersInfos) const
  1209. {
  1210.         real_t totalCost(0.0f);
  1211.         const Vec3 startingLocationInWorldSpace = startingLocation.GetVec3() + meshOrigin;
  1212.         const Vec3 locationInWorldSpace = locationToEval.GetVec3() + meshOrigin;
  1213.         std::for_each(dangersInfos.begin(), dangersInfos.end(), CostAccumulator(locationInWorldSpace, startingLocationInWorldSpace, totalCost));
  1214.         return totalCost;
  1215. }
  1216.  
  1217. void CNavMesh::PullString(const vector3_t& from, const TriangleID fromTriID, const vector3_t& to, const TriangleID toTriID, vector3_t& middlePoint) const
  1218. {
  1219.         if (const TileID fromTileID = ComputeTileID(fromTriID))
  1220.         {
  1221.                 const TileContainer& startContainer = m_tiles[fromTileID - 1];
  1222.                 const STile& startTile = startContainer.tile;
  1223.                 uint16 fromTriangleIdx = ComputeTriangleIndex(fromTriID);
  1224.                 const Tile::STriangle& fromTriangle = startTile.triangles[fromTriangleIdx];
  1225.  
  1226.                 uint16 vi0 = 0, vi1 = 0;
  1227.                 for (int l = 0; l < fromTriangle.linkCount; ++l)
  1228.                 {
  1229.                         const Tile::SLink& link = startTile.links[fromTriangle.firstLink + l];
  1230.                         if (link.side == Tile::SLink::Internal)
  1231.                         {
  1232.                                 TriangleID newTriangleID = ComputeTriangleID(fromTileID, link.triangle);
  1233.                                 if (newTriangleID == toTriID)
  1234.                                 {
  1235.                                         vi0 = link.edge;
  1236.                                         vi1 = next_mod3(link.edge);
  1237.                                         assert(vi0 < 3);
  1238.                                         assert(vi1 < 3);
  1239.                                         break;
  1240.                                 }
  1241.                         }
  1242.                         else
  1243.                         {
  1244.                                 if (link.side != Tile::SLink::OffMesh)
  1245.                                 {
  1246.                                         TileID neighbourTileID = GetNeighbourTileID(startContainer.x, startContainer.y, startContainer.z, link.side);
  1247.                                         TriangleID newTriangleID = ComputeTriangleID(neighbourTileID, link.triangle);
  1248.  
  1249.                                         if (newTriangleID == toTriID)
  1250.                                         {
  1251.                                                 vi0 = link.edge;
  1252.                                                 vi1 = next_mod3(link.edge);
  1253.                                                 assert(vi0 < 3);
  1254.                                                 assert(vi1 < 3);
  1255.                                                 break;
  1256.                                         }
  1257.                                 }
  1258.                         }
  1259.                 }
  1260.  
  1261.                 vector3_t fromVertices[3];
  1262.                 GetVertices(fromTriID, fromVertices[0], fromVertices[1], fromVertices[2]);
  1263.  
  1264.                 if (vi0 != vi1)
  1265.                 {
  1266.                         PREFAST_ASSUME(vi0 < 3 && vi1 < 3);
  1267.  
  1268.                         real_t s, t;
  1269.                         if (!IntersectSegmentSegment(vector2_t(fromVertices[vi0]),
  1270.                                                      vector2_t(fromVertices[vi1]), vector2_t(from), vector2_t(to), s, t))
  1271.                         {
  1272.                                 s = 0;
  1273.                         }
  1274.  
  1275.                         // Even if segments don't intersect, s = 0 and is clamped to the pulling threshold range
  1276.                         s = clamp(s, kMinPullingThreshold, kMaxPullingThreshold);
  1277.  
  1278.                         middlePoint = Lerp(fromVertices[vi0], fromVertices[vi1], s);
  1279.                 }
  1280.         }
  1281. }
  1282.  
  1283. void CNavMesh::AddOffMeshLinkToTile(const TileID tileID, const TriangleID triangleID, const uint16 offMeshIndex)
  1284. {
  1285.         STile& tile = GetTile(tileID);
  1286.  
  1287.         m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::SLink));
  1288.         m_profiler.AddStat(LinkCount, -(int)tile.linkCount);
  1289.  
  1290.         tile.AddOffMeshLink(triangleID, offMeshIndex);
  1291.  
  1292.         m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::SLink));
  1293.         m_profiler.AddStat(LinkCount, tile.linkCount);
  1294. }
  1295.  
  1296. void CNavMesh::UpdateOffMeshLinkForTile(const TileID tileID, const TriangleID triangleID, const uint16 offMeshIndex)
  1297. {
  1298.         STile& tile = GetTile(tileID);
  1299.  
  1300.         tile.UpdateOffMeshLink(triangleID, offMeshIndex);
  1301. }
  1302.  
  1303. void CNavMesh::RemoveOffMeshLinkFromTile(const TileID tileID, const TriangleID triangleID)
  1304. {
  1305.         STile& tile = GetTile(tileID);
  1306.  
  1307.         m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::SLink));
  1308.         m_profiler.AddStat(LinkCount, -(int)tile.linkCount);
  1309.  
  1310.         tile.RemoveOffMeshLink(triangleID);
  1311.  
  1312.         m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::SLink));
  1313.         m_profiler.AddStat(LinkCount, tile.linkCount);
  1314. }
  1315.  
  1316. inline size_t OppositeSide(size_t side)
  1317. {
  1318.         return (side + 7) % 14;
  1319. }
  1320.  
  1321. CNavMesh::ERayCastResult CNavMesh::RayCast(const vector3_t& from, TriangleID fromTri, const vector3_t& to, TriangleID toTri,
  1322.                                            RaycastRequestBase& raycastRequest) const
  1323. {
  1324.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1325.  
  1326.         const bool useNewRaycast = gAIEnv.CVars.MNMRaycastImplementation != 0;
  1327.         if (useNewRaycast)
  1328.         {
  1329.                 return RayCast_new(from, fromTri, to, toTri, raycastRequest);
  1330.         }
  1331.         else
  1332.         {
  1333.                 return RayCast_old(from, fromTri, to, toTri, raycastRequest);
  1334.         }
  1335. }
  1336.  
  1337. struct RaycastNode
  1338. {
  1339.         RaycastNode()
  1340.                 : triangleID(MNM::Constants::InvalidTriangleID)
  1341.                 , percentageOfTotalDistance(-1.0f)
  1342.                 , incidentEdge((uint16) MNM::Constants::InvalidEdgeIndex)
  1343.         {}
  1344.  
  1345.         RaycastNode(const TriangleID _triangleID, const real_t _percentageOfTotalDistance, const uint16 _incidentEdge)
  1346.                 : triangleID(_triangleID)
  1347.                 , percentageOfTotalDistance(_percentageOfTotalDistance)
  1348.                 , incidentEdge(_incidentEdge)
  1349.         {}
  1350.  
  1351.         RaycastNode(const RaycastNode& otherNode)
  1352.                 : triangleID(otherNode.triangleID)
  1353.                 , percentageOfTotalDistance(otherNode.percentageOfTotalDistance)
  1354.                 , incidentEdge(otherNode.incidentEdge)
  1355.         {}
  1356.  
  1357.         ILINE bool IsValid() const { return triangleID != MNM::Constants::InvalidTriangleID; }
  1358.  
  1359.         ILINE bool operator<(const RaycastNode& other) const
  1360.         {
  1361.                 return triangleID < other.triangleID;
  1362.         }
  1363.  
  1364.         ILINE bool operator==(const RaycastNode& other) const
  1365.         {
  1366.                 return triangleID == other.triangleID;
  1367.         }
  1368.  
  1369.         TriangleID triangleID;
  1370.         real_t     percentageOfTotalDistance;
  1371.         uint16     incidentEdge;
  1372. };
  1373.  
  1374. struct IsNodeCloserToEndPredicate
  1375. {
  1376.         bool operator()(const RaycastNode& firstNode, const RaycastNode& secondNode) { return firstNode.percentageOfTotalDistance > secondNode.percentageOfTotalDistance; }
  1377. };
  1378.  
  1379. typedef OpenList<RaycastNode, IsNodeCloserToEndPredicate> RaycastOpenList;
  1380. typedef VectorSet<TriangleID>                             RaycastClosedList;
  1381.  
  1382. CNavMesh::ERayCastResult CNavMesh::RayCast_new(const vector3_t& from, TriangleID fromTriangleID, const vector3_t& to, TriangleID toTriangleID,
  1383.                                                RaycastRequestBase& raycastRequest) const
  1384. {
  1385.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1386.  
  1387.         if (!IsLocationInTriangle(from, fromTriangleID))
  1388.                 return eRayCastResult_InvalidStart;
  1389.  
  1390. #ifdef RAYCAST_DO_NOT_ACCEPT_INVALID_END
  1391.         if (!IsLocationInTriangle(to, toTriangleID))
  1392.                 return eRayCastResult_InvalidEnd;
  1393. #endif
  1394.  
  1395.         RaycastClosedList closedList;
  1396.         closedList.reserve(raycastRequest.maxWayTriCount);
  1397.  
  1398.         RaycastCameFromMap cameFrom;
  1399.         cameFrom.reserve(raycastRequest.maxWayTriCount);
  1400.  
  1401.         const size_t expectedOpenListSize = 10;
  1402.         RaycastOpenList openList(expectedOpenListSize);
  1403.  
  1404.         RaycastNode furtherNodeVisited;
  1405.         RayHit rayHit;
  1406.  
  1407.         RaycastNode currentNode(fromTriangleID, real_t(.0f), (uint16) MNM::Constants::InvalidEdgeIndex);
  1408.  
  1409.         while (currentNode.IsValid())
  1410.         {
  1411.                 if (currentNode.triangleID == toTriangleID)
  1412.                 {
  1413.                         ReconstructRaycastResult(fromTriangleID, toTriangleID, cameFrom, raycastRequest);
  1414.                         return eRayCastResult_NoHit;
  1415.                 }
  1416.  
  1417.                 if (currentNode.percentageOfTotalDistance > furtherNodeVisited.percentageOfTotalDistance)
  1418.                         furtherNodeVisited = currentNode;
  1419.  
  1420.                 rayHit.triangleID = currentNode.triangleID;
  1421.                 RaycastNode nextNode;
  1422.  
  1423.                 closedList.insert(currentNode.triangleID);
  1424.  
  1425.                 TileID tileID = ComputeTileID(currentNode.triangleID);
  1426.                 if (tileID != MNM::Constants::InvalidTileID)
  1427.                 {
  1428.                         const TileContainer* container = &m_tiles[tileID - 1];
  1429.                         const STile* tile = &container->tile;
  1430.  
  1431.                         vector3_t tileOrigin(
  1432.                           real_t(container->x * m_params.tileSize.x),
  1433.                           real_t(container->y * m_params.tileSize.y),
  1434.                           real_t(container->z * m_params.tileSize.z));
  1435.  
  1436.                         const Tile::STriangle& triangle = tile->triangles[ComputeTriangleIndex(currentNode.triangleID)];
  1437.                         for (uint16 edgeIndex = 0; edgeIndex < 3; ++edgeIndex)
  1438.                         {
  1439.                                 if (currentNode.incidentEdge == edgeIndex)
  1440.                                         continue;
  1441.  
  1442.                                 const vector3_t a = tileOrigin + vector3_t(tile->vertices[triangle.vertex[edgeIndex]]);
  1443.                                 const vector3_t b = tileOrigin + vector3_t(tile->vertices[triangle.vertex[inc_mod3[edgeIndex]]]);
  1444.  
  1445.                                 real_t s, t;
  1446.                                 if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(a), vector2_t(b), s, t))
  1447.                                 {
  1448.                                         if (s < currentNode.percentageOfTotalDistance)
  1449.                                                 continue;
  1450.  
  1451.                                         rayHit.distance = s;
  1452.                                         rayHit.edge = edgeIndex;
  1453.  
  1454.                                         for (size_t linkIndex = 0; linkIndex < triangle.linkCount; ++linkIndex)
  1455.                                         {
  1456.                                                 const Tile::SLink& link = tile->links[triangle.firstLink + linkIndex];
  1457.                                                 if (link.edge != edgeIndex)
  1458.                                                         continue;
  1459.  
  1460.                                                 const uint16 side = link.side;
  1461.  
  1462.                                                 if (side == Tile::SLink::Internal)
  1463.                                                 {
  1464.                                                         const Tile::STriangle& opposite = tile->triangles[link.triangle];
  1465.  
  1466.                                                         for (size_t oe = 0; oe < opposite.linkCount; ++oe)
  1467.                                                         {
  1468.                                                                 const Tile::SLink& reciprocal = tile->links[opposite.firstLink + oe];
  1469.                                                                 const TriangleID possibleNextID = ComputeTriangleID(tileID, link.triangle);
  1470.  
  1471.                                                                 if (reciprocal.triangle == ComputeTriangleIndex(currentNode.triangleID))
  1472.                                                                 {
  1473.                                                                         if (closedList.find(possibleNextID) != closedList.end())
  1474.                                                                                 break;
  1475.  
  1476.                                                                         cameFrom[possibleNextID] = currentNode.triangleID;
  1477.  
  1478.                                                                         if (s > currentNode.percentageOfTotalDistance)
  1479.                                                                         {
  1480.                                                                                 nextNode.incidentEdge = reciprocal.edge;
  1481.                                                                                 nextNode.percentageOfTotalDistance = s;
  1482.                                                                                 nextNode.triangleID = possibleNextID;
  1483.                                                                         }
  1484.                                                                         else
  1485.                                                                         {
  1486.                                                                                 RaycastNode possibleNextEdge(possibleNextID, s, reciprocal.edge);
  1487.                                                                                 openList.InsertElement(possibleNextEdge);
  1488.                                                                         }
  1489.  
  1490.                                                                         break;
  1491.                                                                 }
  1492.                                                         }
  1493.                                                 }
  1494.                                                 else if (side != Tile::SLink::OffMesh)
  1495.                                                 {
  1496.                                                         TileID neighbourTileID = GetNeighbourTileID(container->x, container->y, container->z, link.side);
  1497.                                                         const TileContainer& neighbourContainer = m_tiles[neighbourTileID - 1];
  1498.  
  1499.                                                         const Tile::STriangle& opposite = neighbourContainer.tile.triangles[link.triangle];
  1500.  
  1501.                                                         const uint16 currentTriangleIndex = ComputeTriangleIndex(currentNode.triangleID);
  1502.                                                         const uint16 currentOppositeSide = static_cast<uint16>(OppositeSide(side));
  1503.  
  1504.                                                         for (size_t reciprocalLinkIndex = 0; reciprocalLinkIndex < opposite.linkCount; ++reciprocalLinkIndex)
  1505.                                                         {
  1506.                                                                 const Tile::SLink& reciprocal = neighbourContainer.tile.links[opposite.firstLink + reciprocalLinkIndex];
  1507.                                                                 if ((reciprocal.triangle == currentTriangleIndex) && (reciprocal.side == currentOppositeSide))
  1508.                                                                 {
  1509.                                                                         const TriangleID possibleNextID = ComputeTriangleID(neighbourTileID, link.triangle);
  1510.  
  1511.                                                                         if (closedList.find(possibleNextID) != closedList.end())
  1512.                                                                                 break;
  1513.  
  1514.                                                                         const vector3_t neighbourTileOrigin = vector3_t(
  1515.                                                                           real_t(neighbourContainer.x * m_params.tileSize.x),
  1516.                                                                           real_t(neighbourContainer.y * m_params.tileSize.y),
  1517.                                                                           real_t(neighbourContainer.z * m_params.tileSize.z));
  1518.  
  1519.                                                                         const uint16 i0 = reciprocal.edge;
  1520.                                                                         const uint16 i1 = inc_mod3[reciprocal.edge];
  1521.  
  1522.                                                                         assert(i0 < 3);
  1523.                                                                         assert(i1 < 3);
  1524.  
  1525.                                                                         const vector3_t c = neighbourTileOrigin + vector3_t(neighbourContainer.tile.vertices[opposite.vertex[i0]]);
  1526.                                                                         const vector3_t d = neighbourTileOrigin + vector3_t(neighbourContainer.tile.vertices[opposite.vertex[i1]]);
  1527.  
  1528.                                                                         real_t p, q;
  1529.                                                                         if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(c), vector2_t(d), p, q))
  1530.                                                                         {
  1531.                                                                                 cameFrom[possibleNextID] = currentNode.triangleID;
  1532.  
  1533.                                                                                 if (p > currentNode.percentageOfTotalDistance)
  1534.                                                                                 {
  1535.                                                                                         nextNode.incidentEdge = reciprocal.edge;
  1536.                                                                                         nextNode.percentageOfTotalDistance = p;
  1537.                                                                                         nextNode.triangleID = possibleNextID;
  1538.                                                                                 }
  1539.                                                                                 else
  1540.                                                                                 {
  1541.                                                                                         RaycastNode possibleNextEdge(possibleNextID, p, reciprocal.edge);
  1542.                                                                                         openList.InsertElement(possibleNextEdge);
  1543.                                                                                 }
  1544.  
  1545.                                                                                 break;   // reciprocal link loop
  1546.                                                                         }
  1547.                                                                 }
  1548.                                                         }
  1549.                                                 }
  1550.  
  1551.                                                 if (nextNode.IsValid())
  1552.                                                         break;
  1553.                                         }
  1554.  
  1555.                                         if (nextNode.IsValid())
  1556.                                                 break;
  1557.  
  1558.                                 }
  1559.                         }
  1560.                 }
  1561.  
  1562.                 if (nextNode.IsValid())
  1563.                 {
  1564.                         openList.Reset();
  1565.                         currentNode = nextNode;
  1566.                 }
  1567.                 else
  1568.                 {
  1569.                         currentNode = (!openList.IsEmpty()) ? openList.PopBestElement() : RaycastNode();
  1570.                 }
  1571.  
  1572.         }
  1573.  
  1574.         ReconstructRaycastResult(fromTriangleID, furtherNodeVisited.triangleID, cameFrom, raycastRequest);
  1575.  
  1576.         RayHit& requestRayHit = raycastRequest.hit;
  1577.         requestRayHit.distance = rayHit.distance;
  1578.         requestRayHit.triangleID = rayHit.triangleID;
  1579.         requestRayHit.edge = rayHit.edge;
  1580.  
  1581.         return eRayCastResult_Hit;
  1582. }
  1583.  
  1584. CNavMesh::ERayCastResult CNavMesh::ReconstructRaycastResult(const TriangleID fromTriangleID, const TriangleID toTriangleID, const RaycastCameFromMap& cameFrom, RaycastRequestBase& raycastRequest) const
  1585. {
  1586.         TriangleID currentTriangleID = toTriangleID;
  1587.         size_t elementIndex = 0;
  1588.         RaycastCameFromMap::const_iterator elementIterator = cameFrom.find(currentTriangleID);
  1589.         while (elementIterator != cameFrom.end())
  1590.         {
  1591.                 if (elementIndex >= raycastRequest.maxWayTriCount)
  1592.                 {
  1593.                         return eRayCastResult_RayTooLong;
  1594.                 }
  1595.  
  1596.                 raycastRequest.way[elementIndex++] = currentTriangleID;
  1597.                 currentTriangleID = elementIterator->second;
  1598.                 elementIterator = cameFrom.find(currentTriangleID);
  1599.         }
  1600.  
  1601.         raycastRequest.way[elementIndex++] = currentTriangleID;
  1602.         raycastRequest.wayTriCount = elementIndex;
  1603.  
  1604.         return elementIndex < raycastRequest.maxWayTriCount ? eRayCastResult_NoHit : eRayCastResult_RayTooLong;
  1605. }
  1606.  
  1607. bool CNavMesh::IsLocationInTriangle(const vector3_t& location, const TriangleID triangleID) const
  1608. {
  1609.         if (triangleID == MNM::Constants::InvalidTriangleID)
  1610.                 return false;
  1611.  
  1612.         TileID tileID = ComputeTileID(triangleID);
  1613.         if (tileID == MNM::Constants::InvalidTileID)
  1614.                 return false;
  1615.  
  1616.         const TileContainer* container = &m_tiles[tileID - 1];
  1617.         const STile* tile = &container->tile;
  1618.  
  1619.         vector3_t tileOrigin(
  1620.           real_t(container->x * m_params.tileSize.x),
  1621.           real_t(container->y * m_params.tileSize.y),
  1622.           real_t(container->z * m_params.tileSize.z));
  1623.  
  1624.         if (triangleID)
  1625.         {
  1626.                 const Tile::STriangle& triangle = tile->triangles[ComputeTriangleIndex(triangleID)];
  1627.  
  1628.                 const vector2_t a = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[0]]);
  1629.                 const vector2_t b = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[1]]);
  1630.                 const vector2_t c = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[2]]);
  1631.  
  1632.                 return PointInTriangle(vector2_t(location), a, b, c);
  1633.         }
  1634.  
  1635.         return false;
  1636. }
  1637.  
  1638. CNavMesh::ERayCastResult CNavMesh::RayCast_old(const vector3_t& from, TriangleID fromTri, const vector3_t& to, TriangleID toTri, RaycastRequestBase& raycastRequest) const
  1639. {
  1640.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1641.  
  1642.         if (TileID tileID = ComputeTileID(fromTri))
  1643.         {
  1644.                 const TileContainer* container = &m_tiles[tileID - 1];
  1645.                 const STile* tile = &container->tile;
  1646.  
  1647.                 vector3_t tileOrigin(
  1648.                   real_t(container->x * m_params.tileSize.x),
  1649.                   real_t(container->y * m_params.tileSize.y),
  1650.                   real_t(container->z * m_params.tileSize.z));
  1651.  
  1652.                 if (fromTri)
  1653.                 {
  1654.                         const Tile::STriangle& triangle = tile->triangles[ComputeTriangleIndex(fromTri)];
  1655.  
  1656.                         const vector2_t a = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[0]]);
  1657.                         const vector2_t b = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[1]]);
  1658.                         const vector2_t c = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[2]]);
  1659.  
  1660.                         if (!PointInTriangle(vector2_t(from), a, b, c))
  1661.                                 fromTri = 0;
  1662.                 }
  1663.  
  1664.                 if (!fromTri)
  1665.                 {
  1666.                         RayHit& hit = raycastRequest.hit;
  1667.                         hit.distance = -real_t::max();
  1668.                         hit.triangleID = 0;
  1669.                         hit.edge = 0;
  1670.  
  1671.                         return eRayCastResult_Hit;
  1672.                 }
  1673.  
  1674.                 real_t distance = -1;
  1675.                 size_t triCount = 0;
  1676.                 TriangleID currentID = fromTri;
  1677.                 size_t incidentEdge = MNM::Constants::InvalidEdgeIndex;
  1678.  
  1679.                 while (currentID)
  1680.                 {
  1681.                         if (triCount < raycastRequest.maxWayTriCount)
  1682.                         {
  1683.                                 raycastRequest.way[triCount++] = currentID;
  1684.                         }
  1685.                         else
  1686.                         {
  1687.                                 // We don't allow rays that pass through more than maxWayTriCount triangles
  1688.                                 return eRayCastResult_RayTooLong;
  1689.                         }
  1690.  
  1691.                         if (toTri && currentID == toTri)
  1692.                         {
  1693.                                 raycastRequest.wayTriCount = triCount;
  1694.                                 return eRayCastResult_NoHit;
  1695.                         }
  1696.  
  1697.                         const Tile::STriangle& triangle = tile->triangles[ComputeTriangleIndex(currentID)];
  1698.                         TriangleID nextID = 0;
  1699.                         real_t possibleDistance = distance;
  1700.                         size_t possibleIncidentEdge = MNM::Constants::InvalidEdgeIndex;
  1701.                         const TileContainer* possibleContainer = NULL;
  1702.                         const STile* possibleTile = NULL;
  1703.                         vector3_t possibleTileOrigin(tileOrigin);
  1704.                         TileID possibleTileID = tileID;
  1705.  
  1706.                         for (size_t e = 0; e < 3; ++e)
  1707.                         {
  1708.                                 if (incidentEdge == e)
  1709.                                         continue;
  1710.  
  1711.                                 const vector3_t a = tileOrigin + vector3_t(tile->vertices[triangle.vertex[e]]);
  1712.                                 const vector3_t b = tileOrigin + vector3_t(tile->vertices[triangle.vertex[next_mod3(e)]]);
  1713.  
  1714.                                 real_t s, t;
  1715.                                 if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(a), vector2_t(b), s, t))
  1716.                                 {
  1717.                                         if (s < distance)
  1718.                                                 continue;
  1719.  
  1720.                                         for (size_t l = 0; l < triangle.linkCount; ++l)
  1721.                                         {
  1722.                                                 const Tile::SLink& link = tile->links[triangle.firstLink + l];
  1723.                                                 if (link.edge != e)
  1724.                                                         continue;
  1725.  
  1726.                                                 const size_t side = link.side;
  1727.  
  1728.                                                 if (side == Tile::SLink::Internal)
  1729.                                                 {
  1730.                                                         const Tile::STriangle& opposite = tile->triangles[link.triangle];
  1731.  
  1732.                                                         for (size_t oe = 0; oe < opposite.linkCount; ++oe)
  1733.                                                         {
  1734.                                                                 const Tile::SLink& reciprocal = tile->links[opposite.firstLink + oe];
  1735.                                                                 const TriangleID possibleNextID = ComputeTriangleID(tileID, link.triangle);
  1736.                                                                 if (reciprocal.triangle == ComputeTriangleIndex(currentID))
  1737.                                                                 {
  1738.                                                                         distance = s;
  1739.                                                                         nextID = possibleNextID;
  1740.                                                                         possibleIncidentEdge = reciprocal.edge;
  1741.                                                                         possibleTile = tile;
  1742.                                                                         possibleTileOrigin = tileOrigin;
  1743.                                                                         possibleContainer = container;
  1744.                                                                         possibleTileID = tileID;
  1745.  
  1746.                                                                         break;   // opposite edge loop
  1747.                                                                 }
  1748.                                                         }
  1749.                                                 }
  1750.                                                 else if (side != Tile::SLink::OffMesh)
  1751.                                                 {
  1752.                                                         TileID neighbourTileID = GetNeighbourTileID(container->x, container->y, container->z, link.side);
  1753.                                                         const TileContainer& neighbourContainer = m_tiles[neighbourTileID - 1];
  1754.  
  1755.                                                         const Tile::STriangle& opposite = neighbourContainer.tile.triangles[link.triangle];
  1756.  
  1757.                                                         const uint16 currentTriangleIndex = ComputeTriangleIndex(currentID);
  1758.                                                         const uint16 currentOppositeSide = static_cast<uint16>(OppositeSide(side));
  1759.  
  1760.                                                         for (size_t rl = 0; rl < opposite.linkCount; ++rl)
  1761.                                                         {
  1762.                                                                 const Tile::SLink& reciprocal = neighbourContainer.tile.links[opposite.firstLink + rl];
  1763.                                                                 if ((reciprocal.triangle == currentTriangleIndex) && (reciprocal.side == currentOppositeSide))
  1764.                                                                 {
  1765.                                                                         const vector3_t neighbourTileOrigin = vector3_t(
  1766.                                                                           real_t(neighbourContainer.x * m_params.tileSize.x),
  1767.                                                                           real_t(neighbourContainer.y * m_params.tileSize.y),
  1768.                                                                           real_t(neighbourContainer.z * m_params.tileSize.z));
  1769.  
  1770.                                                                         const uint16 i0 = reciprocal.edge;
  1771.                                                                         const uint16 i1 = next_mod3(reciprocal.edge);
  1772.  
  1773.                                                                         assert(i0 < 3);
  1774.                                                                         assert(i1 < 3);
  1775.  
  1776.                                                                         const vector3_t c = neighbourTileOrigin + vector3_t(
  1777.                                                                           neighbourContainer.tile.vertices[opposite.vertex[i0]]);
  1778.                                                                         const vector3_t d = neighbourTileOrigin + vector3_t(
  1779.                                                                           neighbourContainer.tile.vertices[opposite.vertex[i1]]);
  1780.  
  1781.                                                                         const TriangleID possibleNextID = ComputeTriangleID(neighbourTileID, link.triangle);
  1782.                                                                         real_t p, q;
  1783.                                                                         if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(c), vector2_t(d), p, q))
  1784.                                                                         {
  1785.                                                                                 distance = p;
  1786.                                                                                 nextID = possibleNextID;
  1787.                                                                                 possibleIncidentEdge = reciprocal.edge;
  1788.  
  1789.                                                                                 possibleTileID = neighbourTileID;
  1790.                                                                                 possibleContainer = &neighbourContainer;
  1791.                                                                                 possibleTile = &neighbourContainer.tile;
  1792.  
  1793.                                                                                 possibleTileOrigin = neighbourTileOrigin;
  1794.                                                                         }
  1795.  
  1796.                                                                         break;   // reciprocal link loop
  1797.                                                                 }
  1798.                                                         }
  1799.                                                 }
  1800.  
  1801.                                                 if (nextID)
  1802.                                                         break;   // link loop
  1803.                                         }
  1804.                                         if (IsTriangleAlreadyInWay(nextID, raycastRequest.way, triCount))
  1805.                                         {
  1806.                                                 assert(0);
  1807.                                                 nextID = 0;
  1808.                                         }
  1809.  
  1810.                                         // If distance is equals to 0, it means that our starting position is placed
  1811.                                         // on an edge or on a shared vertex. This means that we need to continue evaluating
  1812.                                         // the other edges of the triangle to check if we have a better intersection.
  1813.                                         // This will happen if the ray starts from one edge of the triangle but intersects also
  1814.                                         // on of the other two edges. If we stop to the first intersection (the one with 0 distance)
  1815.                                         // we can end up trying to raycast in the wrong direction.
  1816.                                         // As soon as the distance is more than 0 we can stop the evaluation of the other edges
  1817.                                         // because it means we are moving towards the direction of the ray.
  1818.                                         distance = s;
  1819.                                         const bool shouldStopEvaluationOfOtherEdges = distance > 0;
  1820.                                         if (shouldStopEvaluationOfOtherEdges)
  1821.                                         {
  1822.                                                 if (nextID)
  1823.                                                         break;   // edge loop
  1824.                                                 else
  1825.                                                 {
  1826.                                                         RayHit& hit = raycastRequest.hit;
  1827.                                                         hit.distance = distance;
  1828.                                                         hit.triangleID = currentID;
  1829.                                                         hit.edge = e;
  1830.  
  1831.                                                         raycastRequest.wayTriCount = triCount;
  1832.  
  1833.                                                         return eRayCastResult_Hit;
  1834.                                                 }
  1835.                                         }
  1836.                                 }
  1837.                         }
  1838.  
  1839.                         currentID = nextID;
  1840.                         incidentEdge = possibleIncidentEdge;
  1841.                         tile = possibleTile ? possibleTile : tile;
  1842.                         tileID = possibleTileID;
  1843.                         container = possibleContainer ? possibleContainer : container;
  1844.                         tileOrigin = possibleTileOrigin;
  1845.                 }
  1846.  
  1847.                 raycastRequest.wayTriCount = triCount;
  1848.  
  1849.                 bool isEndingTriangleAcceptable = IsTriangleAcceptableForLocation(to, raycastRequest.way[triCount - 1]);
  1850.                 return isEndingTriangleAcceptable ? eRayCastResult_NoHit : eRayCastResult_Unacceptable;
  1851.         }
  1852.  
  1853.         return eRayCastResult_InvalidStart;
  1854. }
  1855.  
  1856. #pragma warning(push)
  1857. #pragma warning(disable:28285)
  1858. bool TestEdgeOverlap1D(const real_t& a0, const real_t& a1,
  1859.                        const real_t& b0, const real_t& b1, const real_t& dx)
  1860. {
  1861.         if ((a0 == b0) && (a1 == b1))
  1862.                 return true;
  1863.  
  1864.         const real_t amin = a0 < a1 ? a0 : a1;
  1865.         const real_t amax = a0 < a1 ? a1 : a0;
  1866.  
  1867.         const real_t bmin = b0 < b1 ? b0 : b1;
  1868.         const real_t bmax = b0 < b1 ? b1 : b0;
  1869.  
  1870.         const real_t ominx = std::max(amin + dx, bmin + dx);
  1871.         const real_t omaxx = std::min(amax - dx, bmax - dx);
  1872.  
  1873.         if (ominx > omaxx)
  1874.                 return false;
  1875.  
  1876.         return true;
  1877. }
  1878. #pragma warning(pop)
  1879.  
  1880. bool TestEdgeOverlap2D(const real_t& toleranceSq, const vector2_t& a0, const vector2_t& a1,
  1881.                        const vector2_t& b0, const vector2_t& b1, const real_t& dx)
  1882. {
  1883.         if ((a0.x == b0.x) && (a1.x == b1.x) && (a0.x == a1.x))
  1884.                 return TestEdgeOverlap1D(a0.y, a1.y, b0.y, b1.y, dx);
  1885.  
  1886.         const vector2_t amin = a0.x < a1.x ? a0 : a1;
  1887.         const vector2_t amax = a0.x < a1.x ? a1 : a0;
  1888.  
  1889.         const vector2_t bmin = b0.x < b1.x ? b0 : b1;
  1890.         const vector2_t bmax = b0.x < b1.x ? b1 : b0;
  1891.  
  1892.         const real_t ominx = std::max(amin.x, bmin.x) + dx;
  1893.         const real_t omaxx = std::min(amax.x, bmax.x) - dx;
  1894.  
  1895.         if (ominx >= omaxx)
  1896.                 return false;
  1897.  
  1898.         const real_t aslope = ((amax.x - amin.x) != 0) ? ((amax.y - amin.y) / (amax.x - amin.x)) : 0;
  1899.         const real_t a_cValue = amin.y - aslope * amin.x;
  1900.  
  1901.         const real_t bslope = ((bmax.x - bmin.x) != 0) ? ((bmax.y - bmin.y) / (bmax.x - bmin.x)) : 0;
  1902.         const real_t b_cValue = bmin.y - bslope * bmin.x;
  1903.  
  1904.         const real_t aominy = a_cValue + aslope * ominx;
  1905.         const real_t bominy = b_cValue + bslope * ominx;
  1906.  
  1907.         const real_t aomaxy = a_cValue + aslope * omaxx;
  1908.         const real_t bomaxy = b_cValue + bslope * omaxx;
  1909.  
  1910.         const real_t dminy = bominy - aominy;
  1911.         const real_t dmaxy = bomaxy - aomaxy;
  1912.  
  1913.         if ((square(dminy) > toleranceSq) || (square(dmaxy) > toleranceSq))
  1914.         {
  1915.                 return false;
  1916.         }
  1917.  
  1918.         return true;
  1919. }
  1920.  
  1921. bool TestEdgeOverlap(size_t side, const real_t& toleranceSq, const vector3_t& a0, const vector3_t& a1,
  1922.                      const vector3_t& b0, const vector3_t& b1)
  1923. {
  1924.         const int ox = NavMesh::GetNeighbourTileOffset(side)[0];
  1925.         const int oy = NavMesh::GetNeighbourTileOffset(side)[1];
  1926.         const real_t dx = real_t::fraction(1, 1000);
  1927.  
  1928.         if (ox || oy)
  1929.         {
  1930.                 const size_t dim = ox ? 0 : 1;
  1931.                 const size_t odim = dim ^ 1;
  1932.  
  1933.                 if ((a1[dim] - a0[dim] != 0) || (b1[dim] - b0[dim] != 0) || (a0[dim] != b0[dim]))
  1934.                         return false;
  1935.  
  1936.                 return TestEdgeOverlap2D(toleranceSq, vector2_t(a0[odim], a0.z), vector2_t(a1[odim], a1.z),
  1937.                                          vector2_t(b0[odim], b0.z), vector2_t(b1[odim], b1.z), dx);
  1938.         }
  1939.         else
  1940.         {
  1941.                 if ((a1.z - a0.z != 0) || (b1.z - b0.z != 0) || (a0.z != b0.z))
  1942.                         return false;
  1943.  
  1944.                 return TestEdgeOverlap2D(toleranceSq, vector2_t(a0.x, a0.y), vector2_t(a1.x, a1.y),
  1945.                                          vector2_t(b0.x, b0.y), vector2_t(b1.x, b1.y), dx);
  1946.         }
  1947. }
  1948.  
  1949. TileID CNavMesh::SetTile(size_t x, size_t y, size_t z, STile& tile)
  1950. {
  1951.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  1952.  
  1953.         assert((x <= max_x) && (y <= max_y) && (z <= max_z));
  1954.  
  1955.         tile.ValidateTriangles();
  1956.  
  1957.         const size_t tileName = ComputeTileName(x, y, z);
  1958.  
  1959.         std::pair<TileMap::iterator, bool> iresult = m_tileMap.insert(TileMap::value_type(tileName, 0));
  1960.  
  1961.         size_t tileID;
  1962.  
  1963.         if (iresult.second)
  1964.         {
  1965.                 iresult.first->second = tileID = m_tiles.AllocateTileContainer();
  1966.  
  1967.                 m_profiler.AddStat(TileCount, 1);
  1968.         }
  1969.         else
  1970.         {
  1971.                 tileID = iresult.first->second;
  1972.  
  1973.                 STile& oldTile = m_tiles[tileID - 1].tile;
  1974.                 m_triangleCount -= oldTile.triangleCount;
  1975.  
  1976.                 m_profiler.AddStat(VertexCount, -(int)oldTile.vertexCount);
  1977.                 m_profiler.AddStat(TriangleCount, -(int)oldTile.triangleCount);
  1978.                 m_profiler.AddStat(BVTreeNodeCount, -(int)oldTile.nodeCount);
  1979.                 m_profiler.AddStat(LinkCount, -(int)oldTile.linkCount);
  1980.  
  1981.                 m_profiler.FreeMemory(VertexMemory, oldTile.vertexCount * sizeof(Tile::Vertex));
  1982.                 m_profiler.FreeMemory(TriangleMemory, oldTile.triangleCount * sizeof(Tile::STriangle));
  1983.                 m_profiler.FreeMemory(BVTreeMemory, oldTile.nodeCount * sizeof(Tile::SBVNode));
  1984.                 m_profiler.FreeMemory(LinkMemory, oldTile.linkCount * sizeof(Tile::SLink));
  1985.  
  1986.                 oldTile.Destroy();
  1987.         }
  1988.  
  1989.         m_profiler.AddStat(VertexCount, tile.vertexCount);
  1990.         m_profiler.AddStat(TriangleCount, tile.triangleCount);
  1991.         m_profiler.AddStat(BVTreeNodeCount, tile.nodeCount);
  1992.         m_profiler.AddStat(LinkCount, tile.linkCount);
  1993.  
  1994.         m_profiler.AddMemory(VertexMemory, tile.vertexCount * sizeof(Tile::Vertex));
  1995.         m_profiler.AddMemory(TriangleMemory, tile.triangleCount * sizeof(Tile::STriangle));
  1996.         m_profiler.AddMemory(BVTreeMemory, tile.nodeCount * sizeof(Tile::SBVNode));
  1997.         m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::SLink));
  1998.  
  1999.         m_profiler.FreeMemory(GridMemory, m_profiler[GridMemory].used);
  2000.         m_profiler.AddMemory(GridMemory, m_tileMap.size() * sizeof(TileMap::value_type));
  2001.         m_tiles.UpdateProfiler(m_profiler);
  2002.  
  2003.         m_triangleCount += tile.triangleCount;
  2004.  
  2005.         TileContainer& container = m_tiles[tileID - 1];
  2006.         container.x = x;
  2007.         container.y = y;
  2008.         container.z = z;
  2009.         container.tile.Swap(tile);
  2010.         tile.Destroy();
  2011.  
  2012.         return tileID;
  2013. }
  2014.  
  2015. void CNavMesh::ClearTile(TileID tileID, bool clearNetwork)
  2016. {
  2017.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  2018.  
  2019.         {
  2020.                 TileContainer& container = m_tiles[tileID - 1];
  2021.  
  2022.                 m_profiler.AddStat(VertexCount, -(int)container.tile.vertexCount);
  2023.                 m_profiler.AddStat(TriangleCount, -(int)container.tile.triangleCount);
  2024.                 m_profiler.AddStat(BVTreeNodeCount, -(int)container.tile.nodeCount);
  2025.                 m_profiler.AddStat(LinkCount, -(int)container.tile.linkCount);
  2026.  
  2027.                 m_profiler.FreeMemory(VertexMemory, container.tile.vertexCount * sizeof(Tile::Vertex));
  2028.                 m_profiler.FreeMemory(TriangleMemory, container.tile.triangleCount * sizeof(Tile::STriangle));
  2029.                 m_profiler.FreeMemory(BVTreeMemory, container.tile.nodeCount * sizeof(Tile::SBVNode));
  2030.                 m_profiler.FreeMemory(LinkMemory, container.tile.linkCount * sizeof(Tile::SLink));
  2031.  
  2032.                 m_profiler.AddStat(TileCount, -1);
  2033.  
  2034.                 m_triangleCount -= container.tile.triangleCount;
  2035.  
  2036.                 m_tiles.FreeTileContainer(tileID);
  2037.  
  2038.                 container.tile.Destroy();
  2039.  
  2040.                 TileMap::iterator it = m_tileMap.find(ComputeTileName(container.x, container.y, container.z));
  2041.                 assert(it != m_tileMap.end());
  2042.                 m_tileMap.erase(it);
  2043.  
  2044.                 if (clearNetwork)
  2045.                 {
  2046.                         for (size_t side = 0; side < SideCount; ++side)
  2047.                         {
  2048.                                 size_t nx = container.x + NavMesh::GetNeighbourTileOffset(side)[0];
  2049.                                 size_t ny = container.y + NavMesh::GetNeighbourTileOffset(side)[1];
  2050.                                 size_t nz = container.z + NavMesh::GetNeighbourTileOffset(side)[2];
  2051.  
  2052.                                 if (TileID neighbourID = GetTileID(nx, ny, nz))
  2053.                                 {
  2054.                                         TileContainer& ncontainer = m_tiles[neighbourID - 1];
  2055.  
  2056.                                         ReComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, kAdjecencyCalculationToleranceSq, ncontainer.tile,
  2057.                                                            OppositeSide(side), container.x, container.y, container.z, tileID);
  2058.                                 }
  2059.                         }
  2060.                 }
  2061.  
  2062.                 m_profiler.FreeMemory(GridMemory, m_profiler[GridMemory].used);
  2063.                 m_profiler.AddMemory(GridMemory, m_tileMap.size() * sizeof(TileMap::value_type));
  2064.                 m_tiles.UpdateProfiler(m_profiler);
  2065.         }
  2066. }
  2067.  
  2068. void CNavMesh::CreateNetwork()
  2069. {
  2070.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  2071.  
  2072.         TileMap::iterator it = m_tileMap.begin();
  2073.         TileMap::iterator end = m_tileMap.end();
  2074.  
  2075.         const real_t toleranceSq = square(real_t(std::max(m_params.voxelSize.x, m_params.voxelSize.z)));
  2076.  
  2077.         for (; it != end; ++it)
  2078.         {
  2079.                 const TileID tileID = it->second;
  2080.  
  2081.                 TileContainer& container = m_tiles[tileID - 1];
  2082.  
  2083.                 ComputeAdjacency(container.x, container.y, container.z, toleranceSq, container.tile);
  2084.         }
  2085. }
  2086.  
  2087. void CNavMesh::ConnectToNetwork(TileID tileID)
  2088. {
  2089.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  2090.  
  2091.         {
  2092.                 TileContainer& container = m_tiles[tileID - 1];
  2093.  
  2094.                 ComputeAdjacency(container.x, container.y, container.z, kAdjecencyCalculationToleranceSq, container.tile);
  2095.  
  2096.                 for (size_t side = 0; side < SideCount; ++side)
  2097.                 {
  2098.                         size_t nx = container.x + NavMesh::GetNeighbourTileOffset(side)[0];
  2099.                         size_t ny = container.y + NavMesh::GetNeighbourTileOffset(side)[1];
  2100.                         size_t nz = container.z + NavMesh::GetNeighbourTileOffset(side)[2];
  2101.  
  2102.                         if (TileID neighbourID = GetTileID(nx, ny, nz))
  2103.                         {
  2104.                                 TileContainer& ncontainer = m_tiles[neighbourID - 1];
  2105.  
  2106.                                 ReComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, kAdjecencyCalculationToleranceSq, ncontainer.tile,
  2107.                                                    OppositeSide(side), container.x, container.y, container.z, tileID);
  2108.                         }
  2109.                 }
  2110.         }
  2111. }
  2112.  
  2113. TileID CNavMesh::GetTileID(size_t x, size_t y, size_t z) const
  2114. {
  2115.         const size_t tileName = ComputeTileName(x, y, z);
  2116.  
  2117.         TileMap::const_iterator it = m_tileMap.find(tileName);
  2118.         if (it != m_tileMap.end())
  2119.                 return it->second;
  2120.         return 0;
  2121. }
  2122.  
  2123. const STile& CNavMesh::GetTile(TileID tileID) const
  2124. {
  2125.         assert(tileID > 0);
  2126.         return m_tiles[tileID - 1].tile;
  2127. }
  2128.  
  2129. STile& CNavMesh::GetTile(TileID tileID)
  2130. {
  2131.         assert(tileID > 0);
  2132.         return m_tiles[tileID - 1].tile;
  2133. }
  2134.  
  2135. const vector3_t CNavMesh::GetTileContainerCoordinates(TileID tileID) const
  2136. {
  2137.         assert(tileID > 0);
  2138.         const TileContainer& container = m_tiles[tileID - 1];
  2139.         return vector3_t(MNM::real_t(container.x), MNM::real_t(container.y), MNM::real_t(container.z));
  2140. }
  2141.  
  2142. void CNavMesh::Swap(CNavMesh& other)
  2143. {
  2144.         m_tiles.Swap(other.m_tiles);
  2145.         std::swap(m_triangleCount, other.m_triangleCount);
  2146.  
  2147.         m_tileMap.swap(other.m_tileMap);
  2148.  
  2149.         std::swap(m_params, other.m_params);
  2150.         std::swap(m_profiler, other.m_profiler);
  2151. }
  2152.  
  2153. void CNavMesh::Draw(size_t drawFlags, TileID excludeID) const
  2154. {
  2155.         TileMap::const_iterator it = m_tileMap.begin();
  2156.         TileMap::const_iterator end = m_tileMap.end();
  2157.  
  2158.         // collect areas
  2159.         // TODO: Clean this up!  Temprorary to get up and running.
  2160.         std::vector<float> islandAreas(m_islands.size());
  2161.         for (size_t i = 0; i < m_islands.size(); ++i)
  2162.         {
  2163.                 islandAreas[i] = m_islands[i].area;
  2164.         }
  2165.  
  2166.         for (; it != end; ++it)
  2167.         {
  2168.                 if (excludeID == it->second)
  2169.                         continue;
  2170.  
  2171.                 const TileContainer& container = m_tiles[it->second - 1];
  2172.  
  2173.                 container.tile.Draw(drawFlags, vector3_t(
  2174.                                       real_t(m_params.origin.x + container.x * m_params.tileSize.x),
  2175.                                       real_t(m_params.origin.y + container.y * m_params.tileSize.y),
  2176.                                       real_t(m_params.origin.z + container.z * m_params.tileSize.z)),
  2177.                                     it->second,
  2178.                                     islandAreas);
  2179.  
  2180.                 const Vec3 offset = Vec3(0.0f, 0.0f, 0.05f) + m_params.origin +
  2181.                                     Vec3((float)container.x * m_params.tileSize.x,
  2182.                                          (float)container.y * m_params.tileSize.y,
  2183.                                          (float)container.z * m_params.tileSize.z);
  2184.         }
  2185. }
  2186.  
  2187. const CNavMesh::ProfilerType& CNavMesh::GetProfiler() const
  2188. {
  2189.         return m_profiler;
  2190. }
  2191.  
  2192. struct Edge
  2193. {
  2194.         uint16 vertex[2];
  2195.         uint16 triangle[2];
  2196. };
  2197.  
  2198. void ComputeTileTriangleAdjacency(const Tile::STriangle* triangles, const size_t triangleCount, const size_t vertexCount,
  2199.                                   Edge* edges, uint16* adjacency)
  2200. {
  2201.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  2202.  
  2203.         {
  2204.                 enum { Unused = 0xffff, };
  2205.  
  2206.                 const size_t MaxLookUp = 4096;
  2207.                 uint16 edgeLookUp[MaxLookUp];
  2208.                 assert(MaxLookUp > vertexCount + triangleCount * 3);
  2209.  
  2210.                 std::fill(&edgeLookUp[0], &edgeLookUp[0] + vertexCount, static_cast<uint16>(Unused));
  2211.  
  2212.                 uint16 edgeCount = 0;
  2213.  
  2214.                 for (uint16 i = 0; i < triangleCount; ++i)
  2215.                 {
  2216.                         const Tile::STriangle& triangle = triangles[i];
  2217.  
  2218.                         for (size_t v = 0; v < 3; ++v)
  2219.                         {
  2220.                                 uint16 i1 = triangle.vertex[v];
  2221.                                 uint16 i2 = triangle.vertex[next_mod3(v)];
  2222.  
  2223.                                 if (i1 < i2)
  2224.                                 {
  2225.                                         uint16 edgeIdx = edgeCount++;
  2226.                                         adjacency[i * 3 + v] = edgeIdx;
  2227.  
  2228.                                         Edge& edge = edges[edgeIdx];
  2229.                                         edge.triangle[0] = i;
  2230.                                         edge.triangle[1] = i;
  2231.                                         edge.vertex[0] = i1;
  2232.                                         edge.vertex[1] = i2;
  2233.  
  2234.                                         edgeLookUp[vertexCount + edgeIdx] = edgeLookUp[i1];
  2235.                                         edgeLookUp[i1] = edgeIdx;
  2236.                                 }
  2237.                         }
  2238.                 }
  2239.  
  2240.                 for (uint16 i = 0; i < triangleCount; ++i)
  2241.                 {
  2242.                         const Tile::STriangle& triangle = triangles[i];
  2243.  
  2244.                         for (size_t v = 0; v < 3; ++v)
  2245.                         {
  2246.                                 uint16 i1 = triangle.vertex[v];
  2247.                                 uint16 i2 = triangle.vertex[next_mod3(v)];
  2248.  
  2249.                                 if (i1 > i2)
  2250.                                 {
  2251.                                         uint16 edgeIndex = edgeLookUp[i2];
  2252.  
  2253.                                         for (; edgeIndex != Unused; edgeIndex = edgeLookUp[vertexCount + edgeIndex])
  2254.                                         {
  2255.                                                 Edge& edge = edges[edgeIndex];
  2256.  
  2257.                                                 if ((edge.vertex[1] == i1) && (edge.triangle[0] == edge.triangle[1]))
  2258.                                                 {
  2259.                                                         edge.triangle[1] = i;
  2260.                                                         adjacency[i * 3 + v] = edgeIndex;
  2261.                                                         break;
  2262.                                                 }
  2263.                                         }
  2264.  
  2265.                                         if (edgeIndex == Unused)
  2266.                                         {
  2267.                                                 uint16 edgeIdx = edgeCount++;
  2268.                                                 adjacency[i * 3 + v] = edgeIdx;
  2269.  
  2270.                                                 Edge& edge = edges[edgeIdx];
  2271.                                                 edge.vertex[0] = i1;
  2272.                                                 edge.vertex[1] = i2;
  2273.                                                 edge.triangle[0] = i;
  2274.                                                 edge.triangle[1] = i;
  2275.                                         }
  2276.                                 }
  2277.                         }
  2278.                 }
  2279.         }
  2280. }
  2281.  
  2282. TileID CNavMesh::GetNeighbourTileID(size_t x, size_t y, size_t z, size_t side) const
  2283. {
  2284.         size_t nx = x + NavMesh::GetNeighbourTileOffset(side)[0];
  2285.         size_t ny = y + NavMesh::GetNeighbourTileOffset(side)[1];
  2286.         size_t nz = z + NavMesh::GetNeighbourTileOffset(side)[2];
  2287.  
  2288.         return GetTileID(nx, ny, nz);
  2289. }
  2290.  
  2291. void CNavMesh::GetMeshParams(NavMesh::SParams& outParams) const
  2292. {
  2293.         const SGridParams& params = GetGridParams();
  2294.         outParams.originWorld = params.origin;
  2295. }
  2296.  
  2297. TileID CNavMesh::FindTileIDByTileGridCoord(const vector3_t& tileGridCoord) const
  2298. {
  2299.         const int nx = tileGridCoord.x.as_int();
  2300.         const int ny = tileGridCoord.y.as_int();
  2301.         const int nz = tileGridCoord.z.as_int();
  2302.  
  2303.         if (nx < 0 || ny < 0 || nz < 0)
  2304.         {
  2305.                 return Constants::InvalidTileID;
  2306.         }
  2307.  
  2308.         return GetTileID(nx, ny, nz);
  2309. }
  2310.  
  2311. size_t CNavMesh::QueryTriangles(const aabb_t& queryAabbWorld, MNM::NavMesh::IQueryTrianglesFilter* pOptionalFilter, const size_t maxTrianglesCount, TriangleID* pOutTriangles) const
  2312. {
  2313.         CRY_ASSERT(pOutTriangles);
  2314.         CRY_ASSERT(maxTrianglesCount > 0);
  2315.  
  2316.         if (!(pOutTriangles && maxTrianglesCount > 0))
  2317.         {
  2318.                 return 0;
  2319.         }
  2320.  
  2321.         if (pOptionalFilter)
  2322.         {
  2323.                 return QueryTrianglesWithFilterInternal(queryAabbWorld, *pOptionalFilter, maxTrianglesCount, pOutTriangles);
  2324.         }
  2325.         else
  2326.         {
  2327.                 return QueryTrianglesNoFilterInternal(queryAabbWorld, maxTrianglesCount, pOutTriangles);
  2328.         }
  2329. }
  2330.  
  2331. TriangleID CNavMesh::FindClosestTriangle(const vector3_t& queryPosWorld, const TriangleID* pCandidateTriangles, const size_t candidateTrianglesCount, vector3_t* pOutClosestPosWorld, float* pOutClosestDistanceSq) const
  2332. {
  2333.         CRY_ASSERT(pCandidateTriangles);
  2334.         CRY_ASSERT(candidateTrianglesCount > 0);
  2335.  
  2336.         if (!(pCandidateTriangles && candidateTrianglesCount > 0))
  2337.         {
  2338.                 return Constants::InvalidTriangleID;
  2339.         }
  2340.  
  2341.         real_t::unsigned_overflow_type closestDistanceSq;
  2342.         const TriangleID resultTriangleId = FindClosestTriangleInternal(queryPosWorld, pCandidateTriangles, candidateTrianglesCount, pOutClosestPosWorld, &closestDistanceSq);
  2343.         if (resultTriangleId != Constants::InvalidTriangleID)
  2344.         {
  2345.                 if (pOutClosestDistanceSq)
  2346.                 {
  2347.                         // Can't assign overflow_type directly to real_t, but it's still at the same scale, so we can calculate float from it.
  2348.                         // See fixed_t::as_float().
  2349.                         *pOutClosestDistanceSq = (closestDistanceSq / (float)real_t::integer_scale);
  2350.                 }
  2351.         }
  2352.         return resultTriangleId;
  2353. }
  2354.  
  2355. bool CNavMesh::GetTileData(const TileID tileId, Tile::STileData& outTileData) const
  2356. {
  2357.         if (tileId)
  2358.         {
  2359.                 const TileContainer& container = m_tiles[tileId - 1];
  2360.  
  2361.                 const vector3_t origin(
  2362.                   real_t(container.x * m_params.tileSize.x),
  2363.                   real_t(container.y * m_params.tileSize.y),
  2364.                   real_t(container.z * m_params.tileSize.z));
  2365.  
  2366.                 container.tile.GetTileData(outTileData);
  2367.                 outTileData.tileGridCoord = vector3_t(
  2368.                   real_t(container.x),
  2369.                   real_t(container.y),
  2370.                   real_t(container.z));
  2371.                 outTileData.tileOriginWorld = origin;
  2372.  
  2373.                 return true;
  2374.         }
  2375.         return false;
  2376. }
  2377.  
  2378. static const size_t MaxTriangleCount = 1024;
  2379.  
  2380. struct SideTileInfo
  2381. {
  2382.         SideTileInfo()
  2383.                 : tile(0)
  2384.         {
  2385.         }
  2386.  
  2387.         STile*    tile;
  2388.         vector3_t offset;
  2389. };
  2390.  
  2391. #pragma warning (push)
  2392. #pragma warning (disable: 6262)
  2393. void CNavMesh::ComputeAdjacency(size_t x, size_t y, size_t z, const real_t& toleranceSq, STile& tile)
  2394. {
  2395.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  2396.  
  2397.         const size_t vertexCount = tile.vertexCount;
  2398.         const Tile::Vertex* vertices = tile.GetVertices();
  2399.  
  2400.         const size_t triCount = tile.triangleCount;
  2401.         if (!triCount)
  2402.                 return;
  2403.  
  2404.         m_profiler.StartTimer(NetworkConstruction);
  2405.  
  2406.         assert(triCount <= MaxTriangleCount);
  2407.  
  2408.         Edge edges[MaxTriangleCount * 3];
  2409.         uint16 adjacency[MaxTriangleCount * 3];
  2410.         ComputeTileTriangleAdjacency(tile.GetTriangles(), triCount, vertexCount, edges, adjacency);
  2411.  
  2412.         const size_t MaxLinkCount = MaxTriangleCount * 6;
  2413.         Tile::SLink links[MaxLinkCount];
  2414.         size_t linkCount = 0;
  2415.  
  2416.         SideTileInfo sides[SideCount];
  2417.  
  2418.         for (size_t s = 0; s < SideCount; ++s)
  2419.         {
  2420.                 SideTileInfo& side = sides[s];
  2421.  
  2422.                 if (TileID id = GetTileID(x + NavMesh::GetNeighbourTileOffset(s)[0], y + NavMesh::GetNeighbourTileOffset(s)[1], z + NavMesh::GetNeighbourTileOffset(s)[2]))
  2423.                 {
  2424.                         side.tile = &GetTile(id);
  2425.                         side.offset = vector3_t(
  2426.                           NavMesh::GetNeighbourTileOffset(s)[0] * m_params.tileSize.x,
  2427.                           NavMesh::GetNeighbourTileOffset(s)[1] * m_params.tileSize.y,
  2428.                           NavMesh::GetNeighbourTileOffset(s)[2] * m_params.tileSize.z);
  2429.                 }
  2430.         }
  2431.  
  2432.         for (size_t i = 0; i < triCount; ++i)
  2433.         {
  2434.                 size_t triLinkCount = 0;
  2435.  
  2436.                 for (size_t e = 0; e < 3; ++e)
  2437.                 {
  2438.                         const size_t edgeIndex = adjacency[i * 3 + e];
  2439.                         const Edge& edge = edges[edgeIndex];
  2440.                         if ((edge.triangle[0] != i) && (edge.triangle[1] != i))
  2441.                                 continue;
  2442.  
  2443.                         if (edge.triangle[0] != edge.triangle[1])
  2444.                         {
  2445.                                 Tile::SLink& link = links[linkCount++];
  2446.                                 link.side = Tile::SLink::Internal;
  2447.                                 link.edge = e;
  2448.                                 link.triangle = (edge.triangle[1] == i) ? edge.triangle[0] : edge.triangle[1];
  2449.  
  2450.                                 ++triLinkCount;
  2451.                         }
  2452.                 }
  2453.  
  2454.                 if (triLinkCount == 3)
  2455.                 {
  2456.                         Tile::STriangle& triangle = tile.triangles[i];
  2457.                         triangle.linkCount = triLinkCount;
  2458.                         triangle.firstLink = linkCount - triLinkCount;
  2459.  
  2460.                         continue;
  2461.                 }
  2462.  
  2463.                 for (size_t e = 0; e < 3; ++e)
  2464.                 {
  2465.                         const size_t edgeIndex = adjacency[i * 3 + e];
  2466.                         const Edge& edge = edges[edgeIndex];
  2467.                         if ((edge.triangle[0] != i) && (edge.triangle[1] != i))
  2468.                                 continue;
  2469.  
  2470.                         if (edge.triangle[0] != edge.triangle[1])
  2471.                                 continue;
  2472.  
  2473.                         const vector3_t a0 = vector3_t(vertices[edge.vertex[0]]);
  2474.                         const vector3_t a1 = vector3_t(vertices[edge.vertex[1]]);
  2475.  
  2476.                         for (size_t s = 0; s < SideCount; ++s)
  2477.                         {
  2478.                                 if (const STile* stile = sides[s].tile)
  2479.                                 {
  2480.                                         const vector3_t& offset = sides[s].offset;
  2481.  
  2482.                                         const size_t striangleCount = stile->triangleCount;
  2483.                                         const Tile::STriangle* striangles = stile->GetTriangles();
  2484.                                         const Tile::Vertex* svertices = stile->GetVertices();
  2485.  
  2486.                                         for (size_t k = 0; k < striangleCount; ++k)
  2487.                                         {
  2488.                                                 const Tile::STriangle& striangle = striangles[k];
  2489.  
  2490.                                                 for (size_t ne = 0; ne < 3; ++ne)
  2491.                                                 {
  2492.                                                         const vector3_t b0 = offset + vector3_t(svertices[striangle.vertex[ne]]);
  2493.                                                         const vector3_t b1 = offset + vector3_t(svertices[striangle.vertex[next_mod3(ne)]]);
  2494.  
  2495.                                                         if (TestEdgeOverlap(s, toleranceSq, a0, a1, b0, b1))
  2496.                                                         {
  2497.                                                                 Tile::SLink& link = links[linkCount++];
  2498.                                                                 link.side = s;
  2499.                                                                 link.edge = e;
  2500.                                                                 link.triangle = k;
  2501.  
  2502. #if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
  2503.                                                                 const TileID checkId = GetTileID(x + NavMesh::GetNeighbourTileOffset(s)[0], y + NavMesh::GetNeighbourTileOffset(s)[1], z + NavMesh::GetNeighbourTileOffset(s)[2]);
  2504.                                                                 m_tiles.BreakOnInvalidTriangle(ComputeTriangleID(checkId, k));
  2505. #endif
  2506.  
  2507.                                                                 ++triLinkCount;
  2508.                                                                 break;
  2509.                                                         }
  2510.                                                 }
  2511.                                         }
  2512.                                 }
  2513.                         }
  2514.                 }
  2515.  
  2516.                 Tile::STriangle& triangle = tile.triangles[i];
  2517.                 triangle.linkCount = triLinkCount;
  2518.                 triangle.firstLink = linkCount - triLinkCount;
  2519.         }
  2520.  
  2521.         m_profiler.FreeMemory